From: <gb...@us...> - 2008-08-05 17:28:55
|
Revision: 6944 http://playerstage.svn.sourceforge.net/playerstage/?rev=6944&view=rev Author: gbiggs Date: 2008-08-06 00:29:04 +0000 (Wed, 06 Aug 2008) Log Message: ----------- Applied patch 2039151 Modified Paths: -------------- code/player/trunk/server/drivers/mixed/phidgetIFK/CMakeLists.txt code/player/trunk/server/drivers/rfid/CMakeLists.txt Modified: code/player/trunk/server/drivers/mixed/phidgetIFK/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/mixed/phidgetIFK/CMakeLists.txt 2008-08-05 00:27:41 UTC (rev 6943) +++ code/player/trunk/server/drivers/mixed/phidgetIFK/CMakeLists.txt 2008-08-06 00:29:04 UTC (rev 6944) @@ -1,15 +1,15 @@ PLAYERDRIVER_OPTION (phidgetifk build_phidgetifk ON) SET (PHIDGETIFK_DIR "" CACHE STRING "Directory containing the Phidget IFK headers and libraries") MARK_AS_ADVANCED (PHIDGETIFK_DIR) -IF (NOT "${PHIDGETIFK_DIR}" STREQUAL "") +IF ("${PHIDGETIFK_DIR}" STREQUAL "") SET (phidgetReqHeader "phidget21.h") SET (phidgetExtraFlags "") SET (phidgetExtraLibs "-lphidget21") -ELSE (NOT "${PHIDGETIFK_DIR}" STREQUAL "") +ELSE ("${PHIDGETIFK_DIR}" STREQUAL "") SET (phidgetReqHeader "${PHIDGETIFK_DIR}/phidget21.h") SET (phidgetExtraFlags "-I${PHIDGETIFK_DIR}/include") SET (phidgetExtraLibs "-L${PHIDGETIFK_DIR}/lib -lphidget21") -ENDIF (NOT "${PHIDGETIFK_DIR}" STREQUAL "") +ENDIF ("${PHIDGETIFK_DIR}" STREQUAL "") PLAYERDRIVER_REQUIRE_HEADER (phidgetifk build_phidgetifk ${phidgetReqHeader}) PLAYERDRIVER_ADD_DRIVER (phidgetifk build_phidgetifk LINKFLAGS ${phidgetExtraLibs} CFLAGS "${phidgetExtraFlags}" Modified: code/player/trunk/server/drivers/rfid/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/rfid/CMakeLists.txt 2008-08-05 00:27:41 UTC (rev 6943) +++ code/player/trunk/server/drivers/rfid/CMakeLists.txt 2008-08-06 00:29:04 UTC (rev 6944) @@ -10,15 +10,15 @@ PLAYERDRIVER_OPTION (phidgetRFID build_phidgetRFID ON) SET (PHIDGETRFID_DIR "" CACHE STRING "Directory containing the Phidget RFID headers and libraries") MARK_AS_ADVANCED (PHIDGETRFID_DIR) -IF (NOT "${PHIDGETRFID_DIR}" STREQUAL "") +IF ("${PHIDGETRFID_DIR}" STREQUAL "") SET (phidgetReqHeader "phidget21.h") SET (phidgetExtraFlags "") SET (phidgetExtraLibs "-lphidget21") -ELSE (NOT "${PHIDGETRFID_DIR}" STREQUAL "") +ELSE ("${PHIDGETRFID_DIR}" STREQUAL "") SET (phidgetReqHeader "${PHIDGETRFID_DIR}/phidget21.h") SET (phidgetExtraFlags "-I${PHIDGETRFID_DIR}/include") SET (phidgetExtraLibs "-L${PHIDGETRFID_DIR}/lib -lphidget21") -ENDIF (NOT "${PHIDGETRFID_DIR}" STREQUAL "") +ENDIF ("${PHIDGETRFID_DIR}" STREQUAL "") PLAYERDRIVER_REQUIRE_HEADER (phidgetRFID build_phidgetRFID ${phidgetReqHeader}) PLAYERDRIVER_ADD_DRIVER (phidgetRFID build_phidgetRFID LINKFLAGS ${phidgetExtraLibs} CFLAGS "${phidgetExtraFlags}" This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <gb...@us...> - 2008-08-31 19:22:28
|
Revision: 6998 http://playerstage.svn.sourceforge.net/playerstage/?rev=6998&view=rev Author: gbiggs Date: 2008-09-01 02:22:32 +0000 (Mon, 01 Sep 2008) Log Message: ----------- Applied patch 2084625 Modified Paths: -------------- code/player/trunk/server/drivers/rfid/CMakeLists.txt code/player/trunk/server/drivers/wsn/CMakeLists.txt code/player/trunk/server/drivers/wsn/mica2.cc Added Paths: ----------- code/player/trunk/server/drivers/rfid/acr120u.cc code/player/trunk/server/drivers/wsn/phidgetAcc.cc Modified: code/player/trunk/server/drivers/rfid/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/rfid/CMakeLists.txt 2008-09-01 00:57:06 UTC (rev 6997) +++ code/player/trunk/server/drivers/rfid/CMakeLists.txt 2008-09-01 02:22:32 UTC (rev 6998) @@ -23,3 +23,7 @@ PLAYERDRIVER_ADD_DRIVER (phidgetRFID build_phidgetRFID LINKFLAGS ${phidgetExtraLibs} CFLAGS "${phidgetExtraFlags}" SOURCES phidgetRFID.cc) + +PLAYERDRIVER_OPTION (acr120u build_acr120u ON) +PLAYERDRIVER_REQUIRE_HEADER (acr120u build_acr120u "usb.h") +PLAYERDRIVER_ADD_DRIVER (acr120u build_acr120u LINKFLAGS "-lusb" SOURCES acr120u.cc) Added: code/player/trunk/server/drivers/rfid/acr120u.cc =================================================================== --- code/player/trunk/server/drivers/rfid/acr120u.cc (rev 0) +++ code/player/trunk/server/drivers/rfid/acr120u.cc 2008-09-01 02:22:32 UTC (rev 6998) @@ -0,0 +1,415 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2007 + * Federico Ruiz Ugalde ruizf /at/ cs.tum.edu, memeruiz /at/ gmail.com + * Lorenz Moesenlechner moesenle /at/ in.tum.de + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +/** @ingroup drivers */ +/** @{ */ +/** @defgroup driver_acr120u acr_120u + * @brief ACR120U RFID reader + +The acr120u driver communicates with the ACR120U (Part# ACR120U-TK-R, Firmware V2.2U) reader. (13.56MHz Read-Write multitag, anti-collision and USB Powered). + +@par Compile-time dependencies + +- none + +@par Provides + +- @ref interface_rfid + +@par Requires + +- libusb should be installed. + +@par Configuration requests + +- none + +@par Configuration file options + +- sampling_rate (integer) + - Default: 200 + - How often (in mS) should the phidget produce data. Minimum is around 100mS + +- alarmtime (integer) + - Default: 210 + - If the data acquisition cycle takes longer than this time (in mS), a warning will be printed. + +- provides + - The driver supports the "rfid" interface. + - No support for the buzzer and led yet. + +@par Example + +@verbatim +driver +( + name "acr120u" + provides ["rfid:0"] + alwayson 0 + samplingrate 200 + alarmtime 210 +) +@endverbatim + +@author Federico Ruiz Ugalde, Lorenz Moesenlechner + + */ +/** @} */ + + + +#include <libplayercore/playercore.h> +#include <usb.h> + +#include <unistd.h> +#include <string.h> +#include <iostream> +#include <sstream> + +#include <time.h> +#include <sys/time.h> +#include <errno.h> + +//This function returns the difference in mS between two timeval structures +inline float timediffms(struct timeval start, struct timeval end) { + return(end.tv_sec*1000.0 + end.tv_usec/1000.0 - (start.tv_sec*1000.0 + start.tv_usec/1000.0)); +} + +class Acr120u : public Driver { + public: + + Acr120u(ConfigFile* cf, int section); + ~Acr120u(); + + virtual int Setup(); + virtual int Shutdown(); + + virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr * hdr, void * data); + + private: + + // Main function for device thread. + virtual void Main(); + + int intFromHexTuple( char c1, char c2 ) const; + int intFromHexTuple( const char *c ) const { return intFromHexTuple( c[0], c[1] ); } + //!Time between samples (in mS) + float samplingrate; + //!Alarm time (mS) + float alarmtime; + + //!Libusb data + struct usb_bus *busses,*bus,*bus_temp; + usb_dev_handle *HANDLE; + struct usb_device *dev,*dev_temp; + + typedef enum Acr120uCmds { RESET=0, TURN_ON_RADIO, LIST_TAGS }; + static const int Acr120uCmdLength=14; + static const char Acr120uCmdStrings[][Acr120uCmdLength ]; + static const int Acr120uResponseLength=3*8; + static const int vendorId = 0x072f; + static const int productId = 0x8003; + static const int tag_count_position = 20; + static const int tag_startoffset = 1; + + //! Player Interfaces + player_devaddr_t rfid_id; + //player_devaddr_t dio_id; //Use this for the buzzer and led latter +}; + +const char Acr120u::Acr120uCmdStrings[][Acr120u::Acr120uCmdLength ] = +{ + { 0x02, 0x30, 0x31, 0x45, 0x30, 0x30, 0x32, 0x30, 0x35, 0x30, 0x30, 0x45, 0x36, 0x03 }, // Reset + { 0x02, 0x30, 0x31, 0x45, 0x30, 0x30, 0x32, 0x31, 0x33, 0x30, 0x30, 0x46, 0x30, 0x03 }, // Turn on Radio + { 0x02, 0x30, 0x31, 0x45, 0x30, 0x30, 0x32, 0x30, 0x33, 0x30, 0x30, 0x45, 0x30, 0x03 } // List Tags +}; + +int Acr120u::intFromHexTuple( char c1, char c2 ) const +{ + char c_str[] = { c1, c2, 0x00 }; + std::istringstream strm( c_str ); + int num; + + strm.setf( std::ios::hex, std::ios::basefield ); + strm >> num; + + return num; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Constructor. Retrieve options from the configuration file and do any +// pre-Setup() setup. +Acr120u::Acr120u(ConfigFile* cf, int section) + : Driver(cf, section) { + //! Start with a clean device + memset(&rfid_id,0,sizeof(player_devaddr_t)); + //memset(&dio_id,0,sizeof(player_devaddr_t)); //For the buzzer and led + + // Creating the rfid interface + if (cf->ReadDeviceAddr(&(rfid_id), section, "provides", PLAYER_RFID_CODE, -1, NULL) == 0) { + if (AddInterface(rfid_id) != 0) { + SetError(-1); + return; + } + } else { + PLAYER_WARN("rfid interface not created for acr120u driver"); + } + /* + //For the buzzer and led + if (cf->ReadDeviceAddr(&(dio_id), section, "provides", PLAYER_DIO_CODE, -1, NULL) == 0) { + if (AddInterface(dio_id) != 0) { + SetError(-1); + return; + } + } else { + PLAYER_WARN("dio interface not created for phidgetrfid driver"); + }*/ + + + // Set the libusb pointers to NULL + busses=0; + dev=0; + dev_temp=0; + + // Read an option from the configuration file + //serial = cf->ReadInt(section, "serial", -1); + + //Sampling rate and alarm time in mS + samplingrate = cf->ReadFloat(section, "samplingrate", 200.0); + alarmtime = cf->ReadFloat(section, "alarmtime", 210.0); + + return; +} + +Acr120u::~Acr120u() {} + +//////////////////////////////////////////////////////////////////////////////// +// Set up the device. Return 0 if things go well, and -1 otherwise. +int Acr120u::Setup() { + PLAYER_MSG0(1,"ACR120U driver initialising"); + usb_init(); + usb_find_busses(); + usb_find_devices(); + busses=usb_get_busses(); + PLAYER_MSG0(1,"Searching for the device..."); + int found=0; + while ((!found) && busses) { + dev_temp=busses->devices; + while ((!found) && dev_temp) { + if (dev_temp->descriptor.idVendor == 0x072f && dev_temp->descriptor.idProduct == 0x8003) { + dev=dev_temp; + HANDLE=usb_open(dev); + if (usb_claim_interface(HANDLE,0) == 0) { + found=1; + } else { + usb_close(HANDLE); + } + } + dev_temp=dev_temp->next; + } + busses=busses->next; + } + + if (found==1) { + PLAYER_MSG0(1,"Device found. Connection granted to the ACR120U."); + } else { + PLAYER_ERROR("There was a problem connecting to the ACR120u. You don't have device access permissions?"); + return(-1); + } + + //Setting up the device + char acrResponse[Acr120uResponseLength]; + char acrCommand[Acr120uCmdLength]; + //Reset command + memcpy(acrCommand, Acr120uCmdStrings[RESET], Acr120uCmdLength); + usb_control_msg(HANDLE,0x40,0x00,0x00,0x00,acrCommand,Acr120uCmdLength,0); + for (int i=0;i<3;i++) { + usb_interrupt_read(HANDLE,0x01,&acrResponse[i*8],8,0); + } + //Turn on the Radio + memcpy(acrCommand, Acr120uCmdStrings[TURN_ON_RADIO], Acr120uCmdLength); + usb_control_msg(HANDLE,0x40,0x00,0x00,0x00,acrCommand,Acr120uCmdLength,0); + for (int i=0;i<3;i++) { + usb_interrupt_read(HANDLE,0x01,&acrResponse[i*8],8,0); + } + + PLAYER_MSG0(1,"ACR120U driver ready"); + // Start the device thread; spawns a new thread and executes + // Phidgetrfid::Main(), which contains the main loop for the driver. + StartThread(); + + return(0); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Shutdown the device +int Acr120u::Shutdown() { + + PLAYER_MSG0(1,"Shutting ACR120U driver down"); + // Stop and join the driver thread + StopThread(); + //Turn off the device and delete the ACR120U objects + //Turn power off. This is missing yet + //Release interface + usb_clear_halt(HANDLE,0); + usb_clear_halt(HANDLE,1); + usb_release_interface(HANDLE,0); + //Close device + usb_reset(HANDLE); + usb_close(HANDLE); + busses=0; + dev=0; + dev_temp=0; + + PLAYER_MSG0(1,"ACR120U driver has been shutdown"); + + return(0); +} + +int Acr120u::ProcessMessage(QueuePointer &resp_queue, + player_msghdr * hdr, + void * data) { + return(0); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Main function for device thread +void Acr120u::Main() { + + //Need two timers: one for calculating the sleep time to keep a desired framerate. + // The other for measuring the real elapsed time. (And maybe give an alarm) + struct timeval tv_framerate_start; + struct timeval tv_framerate_end; + struct timeval tv_realtime_start; + struct timeval tv_realtime_end; + + gettimeofday( &tv_framerate_start, NULL ); // NULL -> don't want timezone information + tv_realtime_start = tv_framerate_start; + + // The main loop; interact with the device here + while (true) { + //find out the real elapsed time + gettimeofday( &tv_realtime_end, NULL ); + //calculate the time in mS + float real_elapsed=timediffms(tv_realtime_start,tv_realtime_end); + //restart the timer + gettimeofday( &tv_realtime_start, NULL ); + + //check if the time was too long + static bool gavewarning(false); + if ((!gavewarning) && (real_elapsed > alarmtime)) { + PLAYER_WARN2("Cycle took %d mS instead of the desired %d mS. (Only warning once)\n",real_elapsed , samplingrate); + gavewarning=true; + } + + // test if we are supposed to cancel + pthread_testcancel(); + + // Process incoming messages. Phidgetrfid::ProcessMessage() is + // called on each message. + ProcessMessages(); + + //Get the Tags + player_rfid_data_t data_rfid; + char acrCommand[Acr120uCmdLength]; + char acrResponse[Acr120uResponseLength]; + //ListTag + memcpy(acrCommand, Acr120uCmdStrings[LIST_TAGS], Acr120uCmdLength); + usb_control_msg(HANDLE,0x40,0x00,0x00,0x00,acrCommand,14,0); + for (int i=0;i<3;i++) { + usb_interrupt_read(HANDLE,0x01,&acrResponse[i*8],8,0); + } + data_rfid.tags_count=acrResponse[tag_count_position]-0x30; + data_rfid.tags = new player_rfid_tag_t[data_rfid.tags_count+1]; + + //Receiving Tags + for (unsigned int i=0;i<data_rfid.tags_count;i++) { + data_rfid.tags[i].guid = new char[8]; + data_rfid.tags[i].type=1; + data_rfid.tags[i].guid_count=8; + for (int j=0;j<3;j++) { + usb_interrupt_read(HANDLE,0x01,&acrResponse[j*8],8,0); + } + for (int j=0;j<8;j++) { + data_rfid.tags[i].guid[7-j] = intFromHexTuple(&acrResponse[j*2+tag_startoffset]); + } + } + + //Publishing data. + if (rfid_id.interf !=0) { + Publish(rfid_id, PLAYER_MSGTYPE_DATA, PLAYER_RFID_DATA_TAGS, (unsigned char*)&data_rfid, sizeof(player_rfid_data_t), NULL); + } + for (unsigned int i=0;i<data_rfid.tags_count;i++){ + delete [] data_rfid.tags[i].guid; + } + delete [] data_rfid.tags; + + //point to calculate how much to sleep, call nanosleep, after sleep restart the timer + //Get the ammount of time passed: + gettimeofday( &tv_framerate_end, NULL ); + // figure out how much to sleep + long usecs = tv_framerate_end.tv_usec - tv_framerate_start.tv_usec; + long secs = tv_framerate_end.tv_sec - tv_framerate_start.tv_sec; + long elapsed_usecs = 1000000*secs + usecs; + long us_tosleep = static_cast<long>(samplingrate*1000) - elapsed_usecs; + struct timespec ts; + ts.tv_sec = 0; + ts.tv_nsec = us_tosleep*1000; + int done=nanosleep(&ts, NULL); + + //restart the counter + gettimeofday( &tv_framerate_start, NULL ); + + if (done != 0) { + std::cout << "Error in nanosleep! ERRNO: " << errno << " "; + if (errno == EINTR) { + std::cout << "EINTR" ; + } else if (errno == EINVAL) { + std::cout << "EINVAL" ; + } + std::cout << std::endl; + } + + } +} + +// A factory creation function, declared outside of the class so that it +// can be invoked without any object context (alternatively, you can +// declare it static in the class). In this function, we create and return +// (as a generic Driver*) a pointer to a new instance of this driver. +Driver* +Acr120u_Init(ConfigFile* cf, int section) { + // Create and return a new instance of this driver + return((Driver*)(new Acr120u(cf, section))); +} + +// A driver registration function, again declared outside of the class so +// that it can be invoked without object context. In this function, we add +// the driver into the given driver table, indicating which interface the +// driver can support and how to create a driver instance. +void acr120u_Register(DriverTable* table) { + table->AddDriver("acr120u", Acr120u_Init); +} + Modified: code/player/trunk/server/drivers/wsn/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/wsn/CMakeLists.txt 2008-09-01 00:57:06 UTC (rev 6997) +++ code/player/trunk/server/drivers/wsn/CMakeLists.txt 2008-09-01 02:22:32 UTC (rev 6998) @@ -19,3 +19,20 @@ PLAYERDRIVER_OPTION (accel_calib build_accel_calib OFF "STL not found.") ENDIF (HAVE_STL) PLAYERDRIVER_ADD_DRIVER (accel_calib build_accel_calib SOURCES accel_calib.cc) + +PLAYERDRIVER_OPTION (phidgetAcc build_phidgetAcc ON) +SET (PHIDGETACC_DIR "" CACHE STRING "Directory containing the Phidget Acc headers and libraries") +MARK_AS_ADVANCED (PHIDGETACC_DIR) +IF ("${PHIDGETACC_DIR}" STREQUAL "") + SET (phidgetReqHeader "phidget21.h") + SET (phidgetExtraFlags "") + SET (phidgetExtraLibs "-lphidget21") +ELSE ("${PHIDGETACC_DIR}" STREQUAL "") + SET (phidgetReqHeader "${PHIDGETACC_DIR}/phidget21.h") + SET (phidgetExtraFlags "-I${PHIDGETACC_DIR}/include") + SET (phidgetExtraLibs "-L${PHIDGETACC_DIR}/lib -lphidget21") +ENDIF ("${PHIDGETACC_DIR}" STREQUAL "") +PLAYERDRIVER_REQUIRE_HEADER (phidgetAcc build_phidgetAcc ${phidgetReqHeader}) +PLAYERDRIVER_ADD_DRIVER (phidgetAcc build_phidgetAcc + LINKFLAGS ${phidgetExtraLibs} CFLAGS "${phidgetExtraFlags}" + SOURCES phidgetAcc.cc) Modified: code/player/trunk/server/drivers/wsn/mica2.cc =================================================================== --- code/player/trunk/server/drivers/wsn/mica2.cc 2008-09-01 00:57:06 UTC (rev 6997) +++ code/player/trunk/server/drivers/wsn/mica2.cc 2008-09-01 02:22:32 UTC (rev 6998) @@ -129,7 +129,12 @@ #include <sys/ioctl.h> #include <math.h> #include <vector> +#include <iostream> +//For nanosleep: +#include <time.h> +#include <sys/time.h> + // Includes needed for player #include <libplayercore/playercore.h> @@ -159,6 +164,17 @@ virtual void Main (); void RefreshData (); + //!Time between samples (in mS) + float rfidsamplingrate; + //!Alarm time (mS) + float alarmtime; + //Need two timers: one for calculating the sleep time to keep a desired framerate. + // The other for measuring the real elapsed time. (And maybe give an alarm) + struct timeval tv_realtime_start; + struct timeval tv_realtime_end; + float real_elapsed; + bool send_rfidcmd; + // Port file descriptor int fd; @@ -233,6 +249,12 @@ table->AddDriver ("mica2", Mica2_Init); } +//This function returns the difference in mS between two timeval structures +inline float timediffms(struct timeval start, struct timeval end) { + return(end.tv_sec*1000.0 + end.tv_usec/1000.0 - (start.tv_sec*1000.0 + start.tv_usec/1000.0)); +} + + //////////////////////////////////////////////////////////////////////////////// // Constructor. Retrieve options from the configuration file and do any // pre-Setup() setup. @@ -270,6 +292,8 @@ // Filter base node from readings ? filterbasenode = cf->ReadInt (section, "filterbasenode", 0); + rfidsamplingrate = cf->ReadInt (section, "rfidsamplingrate", 500); + // Do we create a WSN interface? if (cf->ReadDeviceAddr (&wsn_addr, section, "provides", @@ -351,8 +375,9 @@ // Set up the device. Return 0 if things go well, and -1 otherwise. int Mica2::Setup () { + real_elapsed=0; // Open serial port - fd = open (port_name, O_RDWR | O_NOCTTY); + fd = open (port_name, O_RDWR | O_NOCTTY | O_NONBLOCK); if (fd < 0) { PLAYER_ERROR2 ("> Connecting to MIB510 on [%s]; [%s]...[failed!]", @@ -425,8 +450,8 @@ // Main function for device thread void Mica2::Main () { - timespec sleepTime = {0, 0}; - + gettimeofday( &tv_realtime_start, NULL ); // NULL -> don't want timezone information + // The main loop; interact with the device here while (true) { @@ -440,8 +465,6 @@ if (base_node_status != 0) // if the base node is asleep, no serial // data can be read RefreshData (); - - nanosleep (&sleepTime, NULL); } } @@ -802,12 +825,24 @@ unsigned char buffer[255]; // Get the time at which we started reading // This will be a pretty good estimate of when the phenomena occured - struct timeval time; - GlobalTime->GetTime (&time); + //find out the real elapsed time + gettimeofday( &tv_realtime_end, NULL ); + //calculate the time in mS + real_elapsed=timediffms(tv_realtime_start,tv_realtime_end)+real_elapsed; + //restart the timer + gettimeofday( &tv_realtime_start, NULL ); + //check if the time was too long + if (real_elapsed > rfidsamplingrate) { + send_rfidcmd=true; + real_elapsed=0; + } + // In case the RFID interface is enabled, send a "select_tag" command first - if ((provideRFID) && (this->rfid_subscriptions > 0)) + if (((provideRFID) && (this->rfid_subscriptions > 0)) && send_rfidcmd) { + send_rfidcmd=false; BuildRFIDHeader (0, NULL, 0, 0xFFFF, 1); + } // Reading from UART length = ReadSerial (buffer); @@ -830,20 +865,26 @@ int err, i = 0; buffer[i] = 0x7e; // serial start byte - while (1) { + int no_read=0; + + while (no_read<100) { err = read (fd, &c, 1); if (err < 0) { - PLAYER_ERROR (">> Error reading from serial port !"); - return (-1); + if (errno!=EAGAIN) { + PLAYER_ERROR (">> Error reading from serial port !"); + return (-1); + } else { no_read++;} } if (err == 1) { + no_read=0; if (++i > 255) return i; buffer[i] = c; if (c == 0x7e) return i; } } + return 0; } //////////////////////////////////////////////////////////////////////////////// @@ -883,11 +924,12 @@ NodeCalibrationValues Mica2::FindNodeValues (unsigned int nodeID) { NodeCalibrationValues n; - NCV::iterator it; - for (it = ncv.begin (); it != ncv.end (); it++) + unsigned int i = 0; + + for (i = 0; i < ncv.size (); i++) { - n = *it; + n = ncv.at (i); if (n.node_id == nodeID) break; } @@ -902,6 +944,8 @@ NodeCalibrationValues node_values; player_wsn_data_t wsn_data; player_rfid_data_t rfid_data; + rfid_data.tags = new player_rfid_tag_t[1]; + rfid_data.tags[0].guid = new char[8]; bool rfidPacket = FALSE; bool wsnPacket = FALSE; int i = 0, o = 2; // index and offset @@ -909,10 +953,6 @@ if ((this->wsn_subscriptions < 1) && (this->rfid_subscriptions < 1)) return -1; - // Zero data - memset (&wsn_data, 0, sizeof (player_wsn_data_t)); - memset (&rfid_data, 0, sizeof (player_rfid_data_t)); - while (i < length) { if (buffer[o] == 0x7d) // handle escape characters @@ -1060,9 +1100,6 @@ rfidPacket = TRUE; - player_rfid_tag_t RFIDtag; - memset (&RFIDtag, 0, sizeof (RFIDtag)); - RFIDMsg *rmsg = (RFIDMsg *)buffer; int dataoffset; @@ -1073,6 +1110,9 @@ response_code <<= 4; response_code &= 0xF0; response_code |= getDigit (rmsg->data[1]); + rfid_data.tags_count = 0; + rfid_data.tags[0].guid_count=0; + rfid_data.tags[0].type=0; if (response_code == 0x14) // SELECT TAG pass { @@ -1080,10 +1120,11 @@ tag_type <<= 4; tag_type &= 0xF0; tag_type |= getDigit (rmsg->data[3]); - RFIDtag.type = tag_type; + rfid_data.tags_count = 1; + rfid_data.tags[0].type = tag_type; dataoffset = 4; - RFIDtag.guid_count = 8; + rfid_data.tags[0].guid_count = 8; int x = 0, cc = 0; int xlength = 23 - (29 - buffer[4]); @@ -1096,13 +1137,10 @@ { char str[3]; sprintf (str, "%c%c", rmsg->data[x], rmsg->data[x+1]); - sscanf (str, "%x", (unsigned int*)&RFIDtag.guid[cc]); + sscanf (str, "%x", (unsigned int*)&rfid_data.tags[0].guid[cc]); cc++; } } - - rfid_data.tags_count = 1; - rfid_data.tags[0] = RFIDtag; } } break; @@ -1121,6 +1159,8 @@ // Write the RFID data Publish (rfid_addr, PLAYER_MSGTYPE_DATA, PLAYER_RFID_DATA_TAGS, &rfid_data, sizeof (player_rfid_data_t), NULL); + delete [] rfid_data.tags[0].guid; + delete [] rfid_data.tags; if ((provideWSN) && (wsnPacket)) // Write the WSN data Added: code/player/trunk/server/drivers/wsn/phidgetAcc.cc =================================================================== --- code/player/trunk/server/drivers/wsn/phidgetAcc.cc (rev 0) +++ code/player/trunk/server/drivers/wsn/phidgetAcc.cc 2008-09-01 02:22:32 UTC (rev 6998) @@ -0,0 +1,351 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2007 Federico Ruiz Ugalde ruizf /at/ cs.tum.edu + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +/** @ingroup drivers */ +/** @{ */ +/** @defgroup driver_phidgetACC phidgetACC + * @brief Phidget ACC + +The phidgetACC driver communicates with the PhidgetACC (Part# 1059) accelerometer. + +@par Compile-time dependencies + +- none + +@par Provides + +- @ref interface_wsn + +@par Requires + +- libphidget from www.phidgets.com should be installed. + +@par Configuration requests + +- none + +@par Configuration file options + +- serial (integer) + - Default: -1 + - This defines which phidget will be controlled if there is more than one connected to the USB bus. + You can obtain the number with lsusb, like this: "lsusb -v |grep iSerial". + The default is -1 , and it will connect to the first phidget available. + +- sampling_rate (integer) + - Default: 40 + - How often (in mS) should the phidget produce data. 40mS produces ACC data at a rate of 25Hz. + +- alarmtime (integer) + - Default: 45 + - If the data acquisition cycle takes longer than this time (in mS), a warning will be printed. + +- provides + - The "wsn" interface with the 3 accelerometers data + +@par Example + +@verbatim +driver +( + name "phidgetAcc" + provides ["wsn:0"] + serial -1 + alwayson 1 + samplingrate 40 + alarmtime 45 +) +@endverbatim + +@author Federico Ruiz Ugalde + + */ +/** @} */ + + + +#include "phidget21.h" +#include <libplayercore/playercore.h> + +#include <unistd.h> +#include <string.h> +#include <iostream> + +//For nanosleep: +#include <time.h> +#include <sys/time.h> +//To catch the errors of nanosleep +#include <errno.h> + +//This function returns the difference in mS between two timeval structures +inline float timediffms(struct timeval start, struct timeval end) { + return(end.tv_sec*1000.0 + end.tv_usec/1000.0 - (start.tv_sec*1000.0 + start.tv_usec/1000.0)); +} + +class PhidgetAcc : public Driver { + public: + + // Constructor; + PhidgetAcc(ConfigFile* cf, int section); + + //Destructor + ~PhidgetAcc(); + + virtual int Setup(); + virtual int Shutdown(); + + virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr * hdr, void * data); + + private: + + // Main function for device thread. + virtual void Main(); + + //!Time between samples (in mS) + float samplingrate; + //!Alarm time (mS) + float alarmtime; + + // WSN interface + player_wsn_data_t data; + + //! Pointer to the ACC Phidget Handle + CPhidgetAccelerometerHandle accel; + + //! Player Interfaces + player_devaddr_t wsn_id; + + //!Serial number of the phidget + int serial; + +// NodeCalibrationValues FindNodeValues (unsigned int nodeID); + player_wsn_data_t DecodePacket (struct p_packet *pkt); + float ConvertAccel (unsigned short raw_accel, int neg_1g, int pos_1g, int converted); + +}; + + +//////////////////////////////////////////////////////////////////////////////// +// Constructor. Retrieve options from the configuration file and do any +// pre-Setup() setup. +PhidgetAcc::PhidgetAcc(ConfigFile* cf, int section) + : Driver(cf, section) { + //! Start with a clean device + memset(&wsn_id,0,sizeof(player_devaddr_t)); + + // Creating the wsn interface + if (cf->ReadDeviceAddr(&(wsn_id), section, "provides", PLAYER_WSN_CODE, -1, NULL) == 0) { + if (AddInterface(wsn_id) != 0) { + SetError(-1); + return; + } + } else { + PLAYER_WARN("wsn interface not created for phidgetAccel driver"); + } + + // Set the phidgetwsn pointer to NULL + accel=0; + + // Read an option from the configuration file + serial = cf->ReadInt(section, "serial", -1); + + //Sampling rate and alarm time in mS + samplingrate = cf->ReadFloat(section, "samplingrate", 40.0); + alarmtime = cf->ReadFloat(section, "alarmtime", 45.0); + + return; +} + +PhidgetAcc::~PhidgetAcc() {} + +//////////////////////////////////////////////////////////////////////////////// +// Set up the device. Return 0 if things go well, and -1 otherwise. +int PhidgetAcc::Setup() { + PLAYER_MSG0(1,"PhidgetAccel driver initialising"); + + //Use the Phidgets library to communicate with the devices + CPhidgetAccelerometer_create(&accel); + CPhidget_open((CPhidgetHandle)accel,serial); + + PLAYER_MSG0(1,"Waiting for Attachment."); + + int status(-1); + + //Wait for attachment 1s or aborts. + status=CPhidget_waitForAttachment((CPhidgetHandle)accel, 1000); + + if (status != 0) { + PLAYER_ERROR("There was a problem connecting to the PhidgetAccelerometer."); + return(1); + } else { + PLAYER_MSG0(1,"Connection granted to the PhidgetAccelerometer."); + } + + PLAYER_MSG0(1,"PhidgetAcc driver ready"); + + // Start the device thread; spawns a new thread and executes + // PhidgetAcc::Main(), which contains the main loop for the driver. + StartThread(); + + return(0); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Shutdown the device +int PhidgetAcc::Shutdown() { + PLAYER_MSG0(1,"Shutting PhidgetAcc driver down"); + + // Stop and join the driver thread + StopThread(); + + // Turn of the device and delete the Phidget objects + CPhidget_close((CPhidgetHandle)accel); + CPhidget_delete((CPhidgetHandle)accel); + accel=0; + + PLAYER_MSG0(1,"PhidgetAcc driver has been shutdown"); + + return(0); +} + +int PhidgetAcc::ProcessMessage(QueuePointer &resp_queue, + player_msghdr * hdr, + void * data) { + return(0); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Main function for device thread +void PhidgetAcc::Main() { + + //Need two timers: one for calculating the sleep time to keep a desired framerate. + // The other for measuring the real elapsed time. (And maybe give an alarm) + struct timeval tv_framerate_start; + struct timeval tv_framerate_end; + struct timeval tv_realtime_start; + struct timeval tv_realtime_end; + + gettimeofday( &tv_framerate_start, NULL ); // NULL -> don't want timezone information + tv_realtime_start = tv_framerate_start; + + // The main loop; interact with the device here + while (true) { + //find out the real elapsed time + gettimeofday( &tv_realtime_end, NULL ); + //calculate the time in mS + float real_elapsed=timediffms(tv_realtime_start,tv_realtime_end); + //restart the timer + gettimeofday( &tv_realtime_start, NULL ); + + //check if the time was too long + static bool gavewarning(false); + if ((!gavewarning) && (real_elapsed > alarmtime)) { + PLAYER_WARN2("Cycle took %d mS instead of the desired %d mS. (Only warning once)\n",real_elapsed , samplingrate); + gavewarning=true; + } + //std::cout << real_elapsed << "mS\n"; + + + // test if we are supposed to cancel + pthread_testcancel(); + + // Process incoming messages. PhidgetAcc::ProcessMessage() is + // called on each message. + ProcessMessages(); + + data.node_type=1; + data.node_id=1; + data.node_parent_id=1; + data.data_packet.light = -1; + data.data_packet.mic = -1; + data.data_packet.magn_x = -1; + data.data_packet.magn_y = -1; + data.data_packet.magn_z = -1; + data.data_packet.temperature = -1; + data.data_packet.battery = -1; + int n_axis; + if(CPhidgetAccelerometer_getNumAxis(accel,&n_axis)) return; + double *p_accel; + p_accel= new double[n_axis]; + for (int i=0;i<n_axis;++i) { + if(CPhidgetAccelerometer_getAcceleration(accel, i, &p_accel[i])) return; + } + data.data_packet.accel_x=p_accel[0]; + data.data_packet.accel_y=p_accel[1]; + data.data_packet.accel_z=p_accel[2]; + delete [] p_accel; + + //Publishing data. + if (wsn_id.interf !=0) { + Publish(wsn_id, PLAYER_MSGTYPE_DATA, PLAYER_WSN_DATA_STATE, (unsigned char*)&data, sizeof(player_wsn_data_t), NULL); + } + + //point to calculate how much to sleep, call nanosleep, after sleep restart the timer + //Get the ammount of time passed: + gettimeofday( &tv_framerate_end, NULL ); + // figure out how much to sleep + long usecs = tv_framerate_end.tv_usec - tv_framerate_start.tv_usec; + long secs = tv_framerate_end.tv_sec - tv_framerate_start.tv_sec; + long elapsed_usecs = 1000000*secs + usecs; + + long us_tosleep = static_cast<long>(samplingrate*1000) - elapsed_usecs; + //cout << "usec to sleep: " << us_tosleep << endl; + + struct timespec ts; + ts.tv_sec = 0; + ts.tv_nsec = us_tosleep*1000; + int done=nanosleep(&ts, NULL); + + //restart the counter + gettimeofday( &tv_framerate_start, NULL ); + + if (done != 0) { + std::cout << "Error in nanosleep! ERRNO: " << errno << " "; + if (errno == EINTR) { + std::cout << "EINTR" ; + } else if (errno == EINVAL) { + std::cout << "EINVAL" ; + } + std::cout << std::endl; + } + + } +} + +// A factory creation function, declared outside of the class so that it +// can be invoked without any object context (alternatively, you can +// declare it static in the class). In this function, we create and return +// (as a generic Driver*) a pointer to a new instance of this driver. +Driver* +PhidgetAcc_Init(ConfigFile* cf, int section) { + // Create and return a new instance of this driver + return((Driver*)(new PhidgetAcc(cf, section))); +} + +// A driver registration function, again declared outside of the class so +// that it can be invoked without object context. In this function, we add +// the driver into the given driver table, indicating which interface the +// driver can support and how to create a driver instance. +void phidgetAcc_Register(DriverTable* table) { + table->AddDriver("phidgetAcc", PhidgetAcc_Init); +} + This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <gb...@us...> - 2008-10-06 01:12:58
|
Revision: 7061 http://playerstage.svn.sourceforge.net/playerstage/?rev=7061&view=rev Author: gbiggs Date: 2008-10-06 01:12:47 +0000 (Mon, 06 Oct 2008) Log Message: ----------- Applied patch 2145102 Modified Paths: -------------- code/player/trunk/server/drivers/mixed/nomad/nomad.cc code/player/trunk/server/drivers/position/motionmind/motionmind.cc Modified: code/player/trunk/server/drivers/mixed/nomad/nomad.cc =================================================================== --- code/player/trunk/server/drivers/mixed/nomad/nomad.cc 2008-10-03 22:25:02 UTC (rev 7060) +++ code/player/trunk/server/drivers/mixed/nomad/nomad.cc 2008-10-06 01:12:47 UTC (rev 7061) @@ -16,10 +16,10 @@ */ #include <unistd.h> -#include <string> +#include <string.h> #include <libplayercore/playercore.h> -#include <cmath> -#include <iostream> +#include <math.h> +#include <stddef.h> #include "Nclient.h" /** @ingroup drivers */ @@ -120,9 +120,6 @@ //TODO add gripper support -using std::cout; -using std::endl; - //constants used in the code const double nomadRadius = 0.225; //nomad radius in meters - measured const double PI = 3.14159265; // aproximate value of pi @@ -220,13 +217,13 @@ //stabilish connection if(! connect_robot(1)) { - cout << "couldn't connect in order to 'zero' the robot... aborting..." << endl; + PLAYER_ERROR ("couldn't connect in order to 'zero' the robot... aborting..."); return; }; - cout << " zeroing the robot position..."; + PLAYER_WARN (" zeroing the robot position..."); zr(); ws(TRUE,TRUE,TRUE,0); - cout << " done" << endl; + PLAYER_WARN (" done"); //close connection to the server disconnect_robot(1); @@ -329,10 +326,9 @@ */ int NomadDriver::Setup () { - cout << endl << "Nomad 200 :: Driver initialising" << endl; - cout << "Nomad 200:: WARNING!!! - make sure there's enough free space around the robot" - << endl; - cout << endl << "Nomad 200 :: Connecting..."; + PLAYER_WARN ("Nomad 200 :: Driver initialising"); + PLAYER_WARN ("Nomad 200:: WARNING!!! - make sure there's enough free space around the robot"); + PLAYER_WARN ("Nomad 200 :: Connecting..."); //setup connection parameters SERV_TCP_PORT = PORT; @@ -340,13 +336,13 @@ //stabilish connection if(! connect_robot(1)) { - cout << "Nomad 200 :: couldn't connect... aborting..." << endl; + PLAYER_ERROR ("Nomad 200 :: couldn't connect... aborting..."); return -1; }; - cout << " done" << endl; + PLAYER_WARN (" done"); //setup nomad atributes - cout << "Nomad 200:: Configuring..."; + PLAYER_WARN ("Nomad 200:: Configuring..."); if (REAL_ROBOT!=3) { if (REAL_ROBOT) { real_robot(); @@ -377,7 +373,7 @@ /// Shutdown the device int NomadDriver::Shutdown () { - cout << endl << "Nomad 200 :: Shutting driver down"; + PLAYER_WARN ("Nomad 200 :: Shutting driver down"); //! Stop and join the driver thread StopThread (); //making sure that the robot stops when shutting down the device @@ -386,7 +382,7 @@ disconnect_robot(1); - cout << endl << "Nomad 200 :: Shutting driver down - DONE" << endl; + PLAYER_WARN ("Nomad 200 :: Shutting driver down - DONE"); return (0); }; Modified: code/player/trunk/server/drivers/position/motionmind/motionmind.cc =================================================================== --- code/player/trunk/server/drivers/position/motionmind/motionmind.cc 2008-10-03 22:25:02 UTC (rev 7060) +++ code/player/trunk/server/drivers/position/motionmind/motionmind.cc 2008-10-06 01:12:47 UTC (rev 7061) @@ -102,7 +102,6 @@ #include <unistd.h> #include <string.h> -#include <iostream> #include <time.h> #include <libplayercore/playercore.h> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2008-10-07 10:24:05
|
Revision: 7073 http://playerstage.svn.sourceforge.net/playerstage/?rev=7073&view=rev Author: thjc Date: 2008-10-07 10:19:43 +0000 (Tue, 07 Oct 2008) Log Message: ----------- Merging changes from 2-1 to trunk: 6975 P2-1 Updated Serial Stream to use file watcher 6979 P2-1 cleaning up some debug messages that are too verbose 6980 P2-1 added socket header for missing defs on some platofrms 6981 P2-1 disabled very verbose debug messages in passthrough, dont think you would ever need it this verbose except while debugging passthrough (when they could be compiled back in) Modified Paths: -------------- code/player/trunk/server/drivers/opaque/serialstream.cc code/player/trunk/server/drivers/opaque/tcpstream.cc code/player/trunk/server/drivers/shell/passthrough.cc Modified: code/player/trunk/server/drivers/opaque/serialstream.cc =================================================================== --- code/player/trunk/server/drivers/opaque/serialstream.cc 2008-10-07 01:22:15 UTC (rev 7072) +++ code/player/trunk/server/drivers/opaque/serialstream.cc 2008-10-07 10:19:43 UTC (rev 7073) @@ -324,7 +324,6 @@ //else if it is a opaque data message then I want to flush the current serial port and write to whatever is connected to the serial port else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_CMD, PLAYER_OPAQUE_CMD_DATA, this->device_addr)) { - PLAYER_MSG0(2, "Command message received"); player_opaque_data_t * recv = reinterpret_cast<player_opaque_data_t * > (data); // Make sure both input and output queues are empty tcflush(opaque_fd, TCIOFLUSH); @@ -365,7 +364,7 @@ for(;;) { // test if we are supposed to cancel - pthread_testcancel(); + Wait(1); // Process incoming messages. SerialStream::ProcessMessage() is // called on each message. @@ -373,9 +372,6 @@ // Reads the data from the serial port and then publishes it ReadData(); - - // Sleep (you might, for example, block on a read() instead) - usleep(100000); } } @@ -406,6 +402,8 @@ usleep(1000); tcflush(opaque_fd, TCIFLUSH); + AddFileWatch(opaque_fd); + return 0; } @@ -498,6 +496,8 @@ // int SerialStream::CloseTerm() { + RemoveFileWatch(opaque_fd); + ::close(this->opaque_fd); return 0; } Modified: code/player/trunk/server/drivers/opaque/tcpstream.cc =================================================================== --- code/player/trunk/server/drivers/opaque/tcpstream.cc 2008-10-07 01:22:15 UTC (rev 7072) +++ code/player/trunk/server/drivers/opaque/tcpstream.cc 2008-10-07 10:19:43 UTC (rev 7073) @@ -103,6 +103,7 @@ #include <string.h> #include <sys/types.h> #include <sys/stat.h> +#include <sys/socket.h> #include <termios.h> #include <unistd.h> #include <sys/ioctl.h> Modified: code/player/trunk/server/drivers/shell/passthrough.cc =================================================================== --- code/player/trunk/server/drivers/shell/passthrough.cc 2008-10-07 01:22:15 UTC (rev 7072) +++ code/player/trunk/server/drivers/shell/passthrough.cc 2008-10-07 10:19:43 UTC (rev 7073) @@ -124,7 +124,7 @@ int ConnectRemote(); int DisconnectRemote(); - + virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr * hdr, void * data); private: @@ -136,16 +136,16 @@ player_devaddr_t srcAddr; //the device that this server connects to to get data Device *srcDevice; - + // properties StringProperty RemoteHost; IntProperty RemotePort; IntProperty RemoteIndex; - + IntProperty Connect; int Connected; - + }; Driver* @@ -206,7 +206,7 @@ int ret = ConnectRemote(); if (ret) return ret; - + } StartThread(); @@ -222,7 +222,7 @@ StopThread(); DisconnectRemote(); - + PLAYER_MSG0(1,"PassThrough driver has been shutdown"); return(0); @@ -232,7 +232,7 @@ { if (Connected) return 0; - + if (RemoteHost.GetValue()[0] != '\0') { PLAYER_MSG1(3,"Overriding remote hostname to %s", RemoteHost.GetValue()); @@ -275,7 +275,7 @@ return 0; //Our clients disconnected, so let's disconnect from our SRC interface srcDevice->Unsubscribe(this->InQueue); - + Connected = 0; return 0; } @@ -289,19 +289,19 @@ bool inspected(false); // let our properties through - if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_SET_STRPROP_REQ)) + if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_SET_STRPROP_REQ)) { player_strprop_req_t req = *reinterpret_cast<player_strprop_req_t*> (data); - if (strcmp("remote_host", req.key) == 0) + if (strcmp("remote_host", req.key) == 0) return -1; } - - if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_SET_INTPROP_REQ)) + + if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_SET_INTPROP_REQ)) { player_intprop_req_t req = *reinterpret_cast<player_intprop_req_t*> (data); - if (strcmp("remote_port", req.key) == 0) + if (strcmp("remote_port", req.key) == 0) return -1; - if (strcmp("remote_index", req.key) == 0) + if (strcmp("remote_index", req.key) == 0) return -1; if (strcmp("connect", req.key) == 0) { @@ -318,11 +318,11 @@ DisconnectRemote(); ConnectRemote(); } - + return -1; } } - + // silence warning etc while we are not connected if (!Connected) { @@ -332,8 +332,8 @@ } return 0; } - + PLAYER_MSG0(9,"PassThrough::ProcessMessage: Received a packet!"); if (Device::MatchDeviceAddress(hdr->addr,srcAddr) && @@ -342,7 +342,7 @@ (hdr->type == PLAYER_MSGTYPE_SYNCH) || (hdr->type == PLAYER_MSGTYPE_RESP_NACK))) { - PLAYER_MSG7(8,"PassThrough: Forwarding SRC->DST Interface code=%d %d:%d:%d -> %d:%d:%d",hdr->addr.interf, hdr->addr.host,hdr->addr.robot, hdr->addr.index, dstAddr.host, dstAddr.robot, dstAddr.index); + //PLAYER_MSG7(8,"PassThrough: Forwarding SRC->DST Interface code=%d %d:%d:%d -> %d:%d:%d",hdr->addr.interf, hdr->addr.host,hdr->addr.robot, hdr->addr.index, dstAddr.host, dstAddr.robot, dstAddr.index); hdr->addr=dstAddr; //will send to my clients, making it seem like it comes from my DST interface @@ -353,7 +353,7 @@ if (Device::MatchDeviceAddress(hdr->addr,dstAddr) && (hdr->type == PLAYER_MSGTYPE_CMD)) { - PLAYER_MSG7(8,"PassThrough: Forwarding DST->SRC Interface code=%d %d:%d:%d -> %d:%d:%d",hdr->addr.interf, hdr->addr.host,hdr->addr.robot, hdr->addr.index, srcAddr.host, srcAddr.robot, srcAddr.index); + //PLAYER_MSG7(8,"PassThrough: Forwarding DST->SRC Interface code=%d %d:%d:%d -> %d:%d:%d",hdr->addr.interf, hdr->addr.host,hdr->addr.robot, hdr->addr.index, srcAddr.host, srcAddr.robot, srcAddr.index); hdr->addr=srcAddr; //send to the device to which I subscribed, making it seem like it comes from my original interface @@ -410,7 +410,7 @@ void PassThrough::Main() { //The forwarding is done in the ProcessMessage method. Called once per each message by ProcessMessages() - while (true) + while (true) { InQueue->Wait(); ProcessMessages(); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <ve...@us...> - 2008-10-10 16:15:23
|
Revision: 7092 http://playerstage.svn.sourceforge.net/playerstage/?rev=7092&view=rev Author: veedee Date: 2008-10-10 16:15:16 +0000 (Fri, 10 Oct 2008) Log Message: ----------- added driver for Videre Design STOC (Stereo on a Chip) cameras Modified Paths: -------------- code/player/trunk/server/drivers/CMakeLists.txt Added Paths: ----------- code/player/trunk/server/drivers/stereo/ code/player/trunk/server/drivers/stereo/CMakeLists.txt code/player/trunk/server/drivers/stereo/stoc.cc Modified: code/player/trunk/server/drivers/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/CMakeLists.txt 2008-10-10 12:33:26 UTC (rev 7091) +++ code/player/trunk/server/drivers/CMakeLists.txt 2008-10-10 16:15:16 UTC (rev 7092) @@ -29,6 +29,7 @@ ADD_SUBDIRECTORY (shell) ADD_SUBDIRECTORY (sonar) ADD_SUBDIRECTORY (speech) +ADD_SUBDIRECTORY (stereo) ADD_SUBDIRECTORY (vectormap) # ADD_SUBDIRECTORY (waveform) ADD_SUBDIRECTORY (wifi) Added: code/player/trunk/server/drivers/stereo/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/stereo/CMakeLists.txt (rev 0) +++ code/player/trunk/server/drivers/stereo/CMakeLists.txt 2008-10-10 16:15:16 UTC (rev 7092) @@ -0,0 +1,2 @@ +PLAYERDRIVER_OPTION (stoc build_stoc ON) +PLAYERDRIVER_ADD_DRIVER (stoc build_stoc LINKFLAGS "-lsvscap -lsvs -lsvscalc -lpthread -ldcap -lraw1394" SOURCES stoc.cc) Added: code/player/trunk/server/drivers/stereo/stoc.cc =================================================================== --- code/player/trunk/server/drivers/stereo/stoc.cc (rev 0) +++ code/player/trunk/server/drivers/stereo/stoc.cc 2008-10-10 16:15:16 UTC (rev 7092) @@ -0,0 +1,710 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2006 Radu Bogdan Rusu (ru...@cs...) + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +/** @ingroup drivers */ +/** @{ */ +/** @defgroup driver_stoc stoc + * @brief STOC + +The stoc driver controls Videre Design's STOC (Stereo on a Chip) cameras +through the SVS (Small Vision System) library. The driver provides a @ref +interface_stereo interface. + +@par Compile-time dependencies + +- none + +@par Provides + +- @ref interface_stereo : left and right images, disparity image, and a 3d + point cloud generated from the disparity + +@par Requires + +- none + +@par Supported configuration requests + + - none + +@par Properties provided + + -none yet + +@par Configuration file options + + - capture_type (integer) + - Default: SVS defaults + - CAP_DUAL or CAP_INTERLACE + + - format (integer) + - Default: SVS defaults + - MONOCHROME, YUV, or RGB24 + + - channel (integer) + - Default: SVS defaults + - 0, 1, 2 etc, video channel on card + + - swap (boolean) + - Default: SVS defaults + - Swapping left/right on or off + + - color_mode (integer) + - Default: SVS defaults + - Color mode: 0 (both), 1 (left only), 2 (right only) + + - color_alg (integer) + - Default: SVS defaults + - Color algorithm (see SVS manual) + + - rectification (boolean) + - Default: SVS defaults + - Image rectification: enabled (1) / disabled (0) + + - proc_mode (integer) + - Default: SVS defaults + - Processing mode for the STOC + + - rate (integer) + - Default: SVS defaults + - Image rate + + - frame_div (integer) + - Default: SVS defaults + - Image sampling + + - image_size (integer typle) + - Default: 640 480 + - Image width and height + + - z_max (float) + - Default: 5 (meters) + - Cutoff distance on Z-axis + + - ndisp (integer) + - Default: SVS defaults + - Set number of disparities, 8 to 128 + + - tex_thresh (integer) + - Default: SVS defaults + - Set texture filter threshold + + - unique (integer) + - Default: SVS defaults + - Set uniqueness filter threshold + + - corrsize (integer) + - Default: SVS defaults + - Set correlation window size, 7 to 21 + + - horopter (integer) + - Default: SVS defaults + - Set horopter (X offset) + + - speckle_size (integer) + - Default: SVS defaults + - Set minimum disparity region size + + - speckle_diff (integer) + - Default: SVS defaults + - Set disparity region neighbor difference + + - cut_di (integer) + - Default: 0 + - Number of lines to "cut" (disconsider) at the bottom of the disparity image + + - multiproc_en (boolean) + - Default: 0 + - Use SVS multiprocessing capabilities (see SVS manual) + +@par Example + +@verbatim +driver +( + name "stoc" + provides ["stereo:0"] + + ## ---[ Misc camera parameters: all -1 values assume SVS defaults + # Color mode: 0 (both), 1 (left only), 2 (right only) + color_mode 0 + # Color algorithm: COLOR_ALG_BEST (2), or COLOR_ALG_FAST (0) - check SVS manual + color_alg 2 + + # Processing mode for STOC: PROC_MODE_OFF (0), PROC_MODE_NONE (1), PROC_MODE_TEST (2), + # PROC_MODE_RECTIFIED (3), PROC_MODE_DISPARITY (4), PROC_MODE_DISPARITY_RAW (5) + # Check your STOC camera manual for extra details. + proc_mode 5 + + # Image rate in fps + rate 15 + + # Image rectification: enabled (1) / disabled (0) + rectification 1 + + ## ---[ Stereo parameters: all -1 values assume SVS defaults + # Points to disconsider at the bottom of the disparity image (due to errors in SVS?) + cut_di 32 + + # Number of disparities, 8 to 128 + ndisp 64 + + # Texture filter threshold (confidence) + tex_thresh 4 + + # Uniqueness filter threshold + unique 3 + + # Correlation window size, 7 to 21 + corrsize 15 + + # Minimum disparity region size + speckle_size 400 +) +@endverbatim + +@author Radu Bogdan Rusu + */ +/** @} */ + +#include <stdlib.h> +#include <libplayercore/playercore.h> +#include <SVS/svsclass.h> + +#define IMAGE_WIDTH 640 +#define IMAGE_HEIGHT 480 + +#define CUTOFF_DIST 5 + +class STOC:public Driver +{ + public: + // constructor + STOC (ConfigFile* cf, int section); + ~STOC (); + + int Setup (); + int Shutdown (); + + // MessageHandler + virtual int ProcessMessage (QueuePointer &resp_queue, + player_msghdr * hdr, + void * data); + + private: + + virtual void Main (); + void RefreshData (); + + // device bookkeeping + player_devaddr_t stereo_addr; + player_stereo_data_t stereo_data; + + int capture_type, format, channel; + bool swap_mode; + int color_mode, color_alg; + int proc_mode, rate, frame_div, size_w, size_h; + bool rectification, multiproc_en; + float z_max; + + int ndisp, tex_thresh, unique, corrsize, horopter, speckle_size, speckle_diff; + int cut_di; + + const char *parameter_file; // user given parameter file for the camera + // SVS objects + svsVideoImages *video; + svsStereoProcess *process; + svsMultiProcess *multiproc; + + protected: + // Properties + IntProperty exposure, balance, gamma, brightness, saturation; + +}; + +//////////////////////////////////////////////////////////////////////////////// +//Factory creation function. This functions is given as an argument when +// the driver is added to the driver table +Driver* + STOC_Init (ConfigFile* cf, int section) +{ + return ((Driver*)(new STOC (cf, section))); +} + +//////////////////////////////////////////////////////////////////////////////// +//Registers the driver in the driver table. Called from the +// player_driver_init function that the loader looks for +void + stoc_Register (DriverTable* table) +{ + table->AddDriver ("stoc", STOC_Init); +} + +//////////////////////////////////////////////////////////////////////////////// +// Constructor. Retrieve options from the configuration file and do any +// pre-Setup() setup. +STOC::STOC (ConfigFile* cf, int section) + : Driver (cf, section), + exposure ("exposure", 0, 0), + balance ("balance", 0, 0), + gamma ("gamma", 0, 0), + brightness ("brightness", 0, 0), + saturation ("saturation", 0, 0) +{ + memset (&this->stereo_addr, 0, sizeof (player_devaddr_t)); + + this->RegisterProperty ("exposure", &this->exposure, cf, section); + this->RegisterProperty ("balance", &this->balance, cf, section); + this->RegisterProperty ("gamma", &this->gamma, cf, section); + this->RegisterProperty ("brightness", &this->brightness, cf, section); + this->RegisterProperty ("saturation", &this->saturation, cf, section); + + // Check whether the user provided a parameter file for the STOC + parameter_file = cf->ReadString (section, "param_file", NULL); + + // SVS camera parameters + capture_type = cf->ReadInt (section, "capture_type", -1); // CAP_DUAL or CAP_INTERLACE + format = cf->ReadInt (section, "format", -1); // MONOCHROME, YUV, or RGB24 + channel = cf->ReadInt (section, "channel", -1); // 0, 1, 2 etc, video channel on card + swap_mode = cf->ReadBool (section, "swap", false); // Swapping left/right on or off + color_mode = cf->ReadInt (section, "color_mode", -1); // Color mode: 0 (both), 1 (left only), 2 (right only) + color_alg = cf->ReadInt (section, "color_alg", -1); // Color algorithm (see SVS manual) + + rectification = cf->ReadBool (section, "rectification", -1); // Image rectification: enabled (1) / disabled (0) + proc_mode = cf->ReadInt (section, "proc_mode", -1); // Processing mode for STOC + rate = cf->ReadInt (section, "rate", -1); // Image rate + frame_div = cf->ReadInt (section, "frame_div", -1); // Image sampling + size_w = cf->ReadTupleInt (section, "image_size", 0, IMAGE_WIDTH); // Image width + size_h = cf->ReadTupleInt (section, "image_size", 1, IMAGE_HEIGHT); // Image height + + // Stereo parameters + z_max = cf->ReadFloat (section, "z_max", CUTOFF_DIST); // Cutoff distance on Z-axis + + ndisp = cf->ReadInt (section, "ndisp", -1); // Set number of disparities, 8 to 128 + tex_thresh = cf->ReadInt (section, "tex_thresh", -1); // Set texture filter threshold + unique = cf->ReadInt (section, "unique", -1); // Set uniqueness filter threshold + corrsize = cf->ReadInt (section, "corrsize", -1); // Set correlation window size, 7 to 21 + horopter = cf->ReadInt (section, "horopter", -1); // Set horopter (X offset) + speckle_size = cf->ReadInt (section, "speckle_size", -1); // Set minimum disparity region size + speckle_diff = cf->ReadInt (section, "speckle_dif", -1); // Set disparity region neighbor difference + + cut_di = cf->ReadInt (section, "cut_di", 0); // Cut points at the bottom of the disparity image + + multiproc_en = cf->ReadBool (section, "multiproc", 0); // SVS multiproc (check manual): enabled (1) / disabled (0) + + if (cf->ReadDeviceAddr (&(this->stereo_addr), section, "provides", PLAYER_STEREO_CODE, -1, NULL) == 0) + { + if (this->AddInterface (this->stereo_addr) != 0) + { + this->SetError (-1); + return; + } + } + + process = new svsStereoProcess (); // Compute the disparity image, and 3D points + multiproc = new svsMultiProcess (); // Multiscale processing + video = getVideoObject (); // Get access to the video stream +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor. +STOC::~STOC () +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Set up the device. Return 0 if things go well, and -1 otherwise. +int + STOC::Setup () +{ + int res; + + int nr_cam = video->Enumerate (); // Get the number of cameras available + + // We only support one camera for now + if (nr_cam == 0) + { + PLAYER_ERROR ("No camera found!"); + return (-1); + } + + for (int i = 0; i < nr_cam; i++) + PLAYER_MSG2 (0, "> Found camera %d: %s", i, video->DeviceIDs ()[i]); + + res = video->Open (nr_cam - 1); // Open camera + + if (!res) + { + PLAYER_ERROR ("Could not connect to camera!"); + return (-1); + } + + /// ---[ Set camera options + if (parameter_file != NULL) + { + video->ReadParams ((char*)parameter_file); + PLAYER_MSG1 (1, ">> Using camera parameters from %s", parameter_file); + } + else + { + if (capture_type != -1) + { + if (video->SetCapture (capture_type)) + PLAYER_MSG1 (2, ">> Set capture type to %d", capture_type); + else + PLAYER_ERROR1 (">> Error while setting capture type to %d!", capture_type); + } + + if (format != -1) + { + if (video->SetFormat (format)) + PLAYER_MSG1 (2, ">> Set format type to %d", format); + else + PLAYER_ERROR1 (">> Error setting format type to %d!", format); + } + + if (channel != -1) + { + if (video->SetChannel (channel)) + PLAYER_MSG1 (2, ">> Set channel type to %d", channel); + else + PLAYER_ERROR1 (">> Error setting channel type to %d!", channel); + } + + if (swap_mode) + { + if (video->SetSwap (swap_mode)) + PLAYER_MSG0 (2, ">> Swap mode enabled"); + else + PLAYER_ERROR (">> Error enabling swap mode!"); + } + + if (color_mode != -1) + { + bool r; + if (color_mode == 0) r = video->SetColor (true, true); + if (color_mode == 1) r = video->SetColor (true, false); + if (color_mode == 2) r = video->SetColor (false, true); + if (r) + PLAYER_MSG1 (2, ">> Color mode set to %d", color_mode); + else + PLAYER_ERROR (">> Error setting color mode to %d!"); + } + + if (color_alg != -1) + { + if (video->SetColorAlg (color_alg)) + PLAYER_MSG1 (2, ">> Color algorithm set to %d", color_alg); + else + PLAYER_ERROR1 (">> Error setting color algorithm to %d!", color_alg); + } + + if (video->SetSize (size_w, size_h)) + PLAYER_MSG2 (2, ">> Image size set to %dx%d", size_w, size_h); + else + PLAYER_ERROR2 (">> Error setting image size to %dx%d!", size_w, size_h); + + if (frame_div != -1) + { + if (video->SetFrameDiv (frame_div)) + PLAYER_MSG1 (2, ">> Image sampling set to %d", frame_div); + else + PLAYER_ERROR1 (">> Error setting image sampling to %d!", frame_div); + } + + if (rate != -1) + { + if (video->SetRate (rate)) + PLAYER_MSG1 (2, ">> Image rate set to %d", rate); + else + PLAYER_ERROR1 (">> Error setting image rate to %d!", rate); + } + + if (proc_mode != -1) + { + if (video->SetProcMode (proc_mode_type(proc_mode))) + PLAYER_MSG1 (2, ">> STOC processing mode set to %d", proc_mode); + else + PLAYER_ERROR1 (">> Error setting STOC processing mode to %d!", proc_mode); + } + + if (rectification != -1) + { + if (video->SetRect (rectification)) + PLAYER_MSG1 (2, ">> Image rectification set to %d", rectification); + else + PLAYER_ERROR1 (">> Error setting image rectification to %d!", rectification); + } + } + video->binning = 1; + + /// ---[ Stereo options + if (cut_di != 0) + PLAYER_MSG1 (2, ">> [stereo] Disconsidering the last %d lines from the bottom of the disparity image...", cut_di); + if (ndisp != -1) + { + video->SetNDisp (ndisp); + PLAYER_MSG1 (2, ">> [stereo] Number of disparities set to %d", ndisp); + } + if (tex_thresh != -1) + { + video->SetThresh (tex_thresh); + PLAYER_MSG1 (2, ">> [stereo] Texture filter threshold set to %d", tex_thresh); + } + if (unique != -1) + { + video->SetUnique (unique); + PLAYER_MSG1 (2, ">> [stereo] Uniqueness filter threshold set to %d", unique); + } + if (corrsize != -1) + { + video->SetCorrsize (corrsize); + PLAYER_MSG1 (2, ">> [stereo] Correlation window size set to %d", corrsize); + } + if (horopter != -1) + { + video->SetHoropter (horopter); + PLAYER_MSG1 (2, ">> [stereo] Horopter (X-Offset) value set to %d", horopter); + } + if (speckle_size != -1) + { + video->SetSpeckleSize (speckle_size); + PLAYER_MSG1 (2, ">> [stereo] Minimum disparity region size set to %d", speckle_size); + } + if (speckle_diff != -1) + { + video->SetSpeckleDiff (speckle_diff); + PLAYER_MSG1 (2, ">> [stereo] Disparity region neighbor diff to %d", speckle_diff); + } + + PLAYER_MSG0 (0, "> Connected to camera"); + + // Start video streaming + res = video->Start (); + + StartThread (); + return (0); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Shutdown the device +int + STOC::Shutdown () +{ + StopThread (); + + int res = video->Stop (); // Stop video streaming + res = video->Close (); // Close camera + PLAYER_MSG0 (1, "> Closed camera connection."); + return (0); +} + +//////////////////////////////////////////////////////////////////////////////// +// ProcessMessage +int + STOC::ProcessMessage (QueuePointer &resp_queue, + player_msghdr * hdr, + void * data) +{ + assert (hdr); + assert (data); + + // To do: offset, exposure, autoexposure parameters, balance, gamma, brightness, saturation + // level and horopter + + return (0); +} + + + +//////////////////////////////////////////////////////////////////////////////// +void + STOC::Main () +{ + for (;;) + { + // handle commands and requests/replies -------------------------------- + pthread_testcancel (); + ProcessMessages (); + RefreshData (); + } +} + +//////////////////////////////////////////////////////////////////////////////// +// RefreshData function +void + STOC::RefreshData () +{ + int res; + + svsStereoImage *si = video->GetImage (10); // 10 ms timeout + if (si == NULL) + PLAYER_WARN ("No image, timed out..."); + + // Compute the disparity image + if (multiproc_en) + multiproc->CalcStereo (si); + else + process->CalcStereo (si); + + // Trim down those nasty points at the bottom of the disparity image (errors?) + for (int i = si->ip.height - cut_di; i < si->ip.height; i++) + for (int j = 0; j < si->ip.width; j++) + si->disparity[i * si->ip.width + j] = -2; + + // Compute the 3D point cloud information + if (multiproc_en) + multiproc->Calc3D (si, 0, 0, 0, 0, NULL, NULL, z_max); + else + process->Calc3D (si, 0, 0, 0, 0, NULL, NULL, z_max); + + // Save the 3D point cloud data in the structure, if present + int nr_points = 0; + stereo_data.points = NULL; + if (si->have3D) + { + stereo_data.points = new player_pointcloud3d_stereo_element_t[si->ip.height * si->ip.width]; + + svs3Dpoint *pts = si->pts3D; + for (int i = 0; i < si->ip.height; i++) + { + for (int j = 0; j < si->ip.width; j++, pts++) + { + // Check if the point is valid + if (pts->A <= 0) + continue; + + player_pointcloud3d_stereo_element_t point; + point.px = pts->X; + point.py = pts->Y; + point.pz = pts->Z; + + if (si->haveColor) + { + svsColorPixel *mpc = (svsColorPixel*)(si->color + (i*si->ip.width + j) * 4); + point.red = mpc->r; // red + point.green = mpc->g; // green + point.blue = mpc->b; // blue + } + else + { + point.red = point.green = point.blue = (unsigned char)si->Left ()[i*si->ip.width + j]; + } + stereo_data.points[nr_points] = point; + nr_points++; + } // width + } // height + + } // have3D + stereo_data.points_count = nr_points; + + // Save the stereo left/right image + stereo_data.left_channel.height = stereo_data.right_channel.height = si->ip.height; + stereo_data.left_channel.width = stereo_data.right_channel.width = si->ip.width; + stereo_data.left_channel.fdiv = stereo_data.right_channel.fdiv = frame_div; + stereo_data.left_channel.compression = stereo_data.right_channel.compression = PLAYER_CAMERA_COMPRESS_RAW; + stereo_data.left_channel.image_count = stereo_data.left_channel.width * stereo_data.left_channel.height; + stereo_data.right_channel.image_count = stereo_data.left_channel.width * stereo_data.left_channel.height; + + // Set MONO by default on both left and right + stereo_data.left_channel.format = PLAYER_CAMERA_FORMAT_MONO8; + stereo_data.left_channel.bpp = 8; + stereo_data.left_channel.image = si->Left (); + stereo_data.right_channel.format = PLAYER_CAMERA_FORMAT_MONO8; + stereo_data.right_channel.bpp = 8; + stereo_data.right_channel.image = si->Right (); + + uint8_t *in_left, *in_right, *out_left, *out_right; + // Check if <left> has color or monochrome + if (si->haveColor) + { + stereo_data.left_channel.format = PLAYER_CAMERA_FORMAT_RGB888; + stereo_data.left_channel.bpp = 24; + stereo_data.left_channel.image_count *= 3; + in_left = (uint8_t*)si->Color (); + stereo_data.left_channel.image = new unsigned char [stereo_data.left_channel.image_count]; + out_left = (uint8_t*)stereo_data.left_channel.image; + } + + // Check if <right> has color or monochrome + if (si->haveColorRight) + { + stereo_data.right_channel.format = PLAYER_CAMERA_FORMAT_RGB888; + stereo_data.right_channel.bpp = 24; + stereo_data.right_channel.image_count *= 3; + in_right = (uint8_t*)si->ColorRight (); + stereo_data.right_channel.image = new unsigned char [stereo_data.right_channel.image_count]; + out_right = (uint8_t*)stereo_data.right_channel.image; + } + + for (int i = 0; i < (si->ip.height * si->ip.width); i++) + { + if (si->haveColor) + { + // Left + out_left[0] = in_left[0]; + out_left[1] = in_left[1]; + out_left[2] = in_left[2]; + out_left += 3; + in_left += 4; + } + if (si->haveColorRight) + { + // Right + out_right[0] = in_right[0]; + out_right[1] = in_right[1]; + out_right[2] = in_right[2]; + out_right += 3; + in_right += 4; + } + } + + // Save the disparity image + stereo_data.disparity.width = stereo_data.disparity.height = 0; + stereo_data.disparity.bpp = 16; + stereo_data.disparity.fdiv = 1; + stereo_data.disparity.compression = PLAYER_CAMERA_COMPRESS_RAW; + stereo_data.disparity.format = PLAYER_CAMERA_FORMAT_MONO16; + + if (si->haveDisparity) + { + stereo_data.disparity.width = si->dp.dwidth; + stereo_data.disparity.height = si->dp.dheight; + stereo_data.disparity.image = (uint8_t*)si->disparity; + } + stereo_data.disparity.image_count = stereo_data.disparity.width * stereo_data.disparity.height * 2; + + // Stereo data mode + stereo_data.mode = 1 + 2 + 4; // left + right + disparity + + // Publish the stereo data + Publish (stereo_addr, PLAYER_MSGTYPE_DATA, PLAYER_STEREO_DATA_STATE, (void*)&stereo_data); + + // Cleanup + if (stereo_data.points != NULL) + delete [] stereo_data.points; + if (si->haveColor) + delete [] stereo_data.left_channel.image; + if (si->haveColorRight) + delete [] stereo_data.right_channel.image; + + return; +} This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2009-01-04 02:22:15
|
Revision: 7238 http://playerstage.svn.sourceforge.net/playerstage/?rev=7238&view=rev Author: thjc Date: 2009-01-04 02:22:13 +0000 (Sun, 04 Jan 2009) Log Message: ----------- applied patch 2101500 for motionmind Modified Paths: -------------- code/player/trunk/server/drivers/opaque/tcpstream.cc code/player/trunk/server/drivers/position/motionmind/motionmind.cc Modified: code/player/trunk/server/drivers/opaque/tcpstream.cc =================================================================== --- code/player/trunk/server/drivers/opaque/tcpstream.cc 2009-01-04 02:10:05 UTC (rev 7237) +++ code/player/trunk/server/drivers/opaque/tcpstream.cc 2009-01-04 02:22:13 UTC (rev 7238) @@ -360,6 +360,11 @@ CloseTerm(); return; } + + if (len == buffer_size) + { + PLAYER_WARN("tcpstream:ReadData() filled entire buffer, increasing buffer size will lower latency"); + } assert(len <= int(buffer_size)); mData.data_count = len; Modified: code/player/trunk/server/drivers/position/motionmind/motionmind.cc =================================================================== --- code/player/trunk/server/drivers/position/motionmind/motionmind.cc 2009-01-04 02:10:05 UTC (rev 7237) +++ code/player/trunk/server/drivers/position/motionmind/motionmind.cc 2009-01-04 02:22:13 UTC (rev 7238) @@ -90,6 +90,8 @@ parity "none" provides ["opaque:0"] alwayson 1 + # IF ATTACHED TO MORE THAN ONE MOTIONMIND BOARD YOU MUST HAVE A WAIT TIME OR THINGS TIMEOUT + wait_time 40000 ) @endverbatim @@ -106,10 +108,10 @@ #include <libplayercore/playercore.h> -#define DEFAULT_RX_BUFFER_SIZE 500*1024/8 +#define DEFAULT_RX_BUFFER_SIZE 128 #define DEFAULT_ADDRESS 1 #define MESSAGE_LENGTH 7 -#define MSG_TIMEOUT 0.1 //Seconds before it sends another read request if it hasn't yet got a reply +#define MSG_TIMEOUT 0.25 //Seconds before it sends another read request if it hasn't yet got a reply extern PlayerTime *GlobalTime; @@ -291,7 +293,6 @@ else { memcpy(&rx_buffer[messageOffset], recv->data, recv->data_count); - //TODO: addd do stuff here to deal with the new data } return 0; } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2009-01-24 23:14:26
|
Revision: 7297 http://playerstage.svn.sourceforge.net/playerstage/?rev=7297&view=rev Author: thjc Date: 2009-01-24 23:14:21 +0000 (Sat, 24 Jan 2009) Log Message: ----------- Many fixes to threaded drivers, mostly shutdown methods replaced with MainQuit Modified Paths: -------------- code/player/trunk/server/drivers/base/imagebase.cc code/player/trunk/server/drivers/base/imagebase.h code/player/trunk/server/drivers/blobfinder/simpleshape/simpleshape.cc code/player/trunk/server/drivers/blobfinder/upcbarcode/upcbarcode.cc code/player/trunk/server/drivers/camera/sphere/sphere_mixed.cc code/player/trunk/server/drivers/camera/sphere/sphere_mixed.h code/player/trunk/server/drivers/camera/yarp/YarpImage.cc code/player/trunk/server/drivers/joystick/linuxjoy.cc code/player/trunk/server/drivers/laser/RS4LeuzeLaserDriver.cc code/player/trunk/server/drivers/laser/bumper2laser.cc code/player/trunk/server/drivers/laser/sicklms200.cc code/player/trunk/server/drivers/localization/amcl/amcl_laser.cc code/player/trunk/server/drivers/mixed/chatterbox/cb_driver.cc code/player/trunk/server/drivers/mixed/cmucam2/cmucam2.cc code/player/trunk/server/drivers/mixed/erratic/erratic.cc code/player/trunk/server/drivers/mixed/erratic/erratic.h code/player/trunk/server/drivers/mixed/evolution/er1/er.cc code/player/trunk/server/drivers/mixed/evolution/er1/er.h code/player/trunk/server/drivers/mixed/garcia/garcia_mixed.cc code/player/trunk/server/drivers/mixed/garcia/garcia_mixed.h code/player/trunk/server/drivers/mixed/irobot/create/create_driver.cc code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_driver.cc code/player/trunk/server/drivers/mixed/mricp/src/mricp_driver.cpp code/player/trunk/server/drivers/mixed/p2os/p2os.cc code/player/trunk/server/drivers/mixed/p2os/p2os.h code/player/trunk/server/drivers/mixed/phidgetIFK/phidgetIFK.cc code/player/trunk/server/drivers/mixed/reb/reb.cc code/player/trunk/server/drivers/mixed/reb/reb.h code/player/trunk/server/drivers/mixed/rflex/rflex.cc code/player/trunk/server/drivers/mixed/rflex/rflex.h code/player/trunk/server/drivers/mixed/rmp/segwayrmp.cc code/player/trunk/server/drivers/mixed/rmp/segwayrmp.h code/player/trunk/server/drivers/mixed/robotino/robotino_driver.cc code/player/trunk/server/drivers/mixed/sr3000/sr3000.cc code/player/trunk/server/drivers/mixed/wbr/914/wbr914.cc code/player/trunk/server/drivers/mixed/wbr/914/wbr914.h code/player/trunk/server/drivers/opaque/flexiport.cc code/player/trunk/server/drivers/opaque/serialstream.cc code/player/trunk/server/drivers/opaque/tcpstream.cc code/player/trunk/server/drivers/planner/wavefront/wavefront.cc code/player/trunk/server/drivers/position/ascension/flockofbirds.cc code/player/trunk/server/drivers/position/isense/inertiacube2.cc code/player/trunk/server/drivers/position/mbicp/mbicp_driver.cc code/player/trunk/server/drivers/position/microstrain/3dmg.cc code/player/trunk/server/drivers/position/roboteq/roboteq.cc code/player/trunk/server/drivers/position/vfh/vfh.cc code/player/trunk/server/drivers/ptz/canonvcc4.cc code/player/trunk/server/drivers/ptz/ptu46.cc code/player/trunk/server/drivers/ptz/sonyevid30.cc code/player/trunk/server/drivers/ranger/gbxsickacfr.cc code/player/trunk/server/drivers/rfid/phidgetRFID.cc code/player/trunk/server/drivers/service_adv/mdns.cc code/player/trunk/server/drivers/shell/kartowriter.cc code/player/trunk/server/drivers/shell/readlog.cc code/player/trunk/server/drivers/shell/writelog.cc code/player/trunk/server/drivers/speech/festival.cc code/player/trunk/server/drivers/vectormap/vec2map.cc code/player/trunk/server/drivers/wifi/iwspy.cc code/player/trunk/server/drivers/wsn/mica2.cc Modified: code/player/trunk/server/drivers/base/imagebase.cc =================================================================== --- code/player/trunk/server/drivers/base/imagebase.cc 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/base/imagebase.cc 2009-01-24 23:14:21 UTC (rev 7297) @@ -98,13 +98,15 @@ PLAYER_ERROR ("unable to subscribe to camera device"); return -1; } - - - camera_driver->Unsubscribe(InQueue); - return 0; } +void ImageBase::MainQuit() +{ + if (camera_driver) + camera_driver->Unsubscribe(InQueue); +} + //////////////////////////////////////////////////////////////////////////////// // Process an incoming message int ImageBase::ProcessMessage (QueuePointer &resp_queue, player_msghdr * hdr, void * data) Modified: code/player/trunk/server/drivers/base/imagebase.h =================================================================== --- code/player/trunk/server/drivers/base/imagebase.h 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/base/imagebase.h 2009-01-24 23:14:21 UTC (rev 7297) @@ -1,7 +1,7 @@ /* * Player - One Hell of a Robot Server * Copyright (C) 2000 Brian Gerkey et al - * ge...@us... + * ge...@us... * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -37,7 +37,7 @@ @par Requires - This driver acquires image data from a @ref interface_camera - interface. + interface. @par Provides @@ -61,7 +61,7 @@ // Driver for detecting laser retro-reflectors. class ImageBase : public ThreadedDriver { - public: + public: // Constructor ImageBase(ConfigFile *cf, int section, bool overwrite_cmds, size_t queue_maxlen, int interf); ImageBase(ConfigFile *cf, int section, bool overwrite_cmds = true, size_t queue_maxlen = PLAYER_MSGQUEUE_DEFAULT_MAXLEN); @@ -71,19 +71,20 @@ PLAYER_WARN("image deleted from the memory"); } - // Process incoming messages from clients + // Process incoming messages from clients int ProcessMessage (QueuePointer &resp_queue, player_msghdr * hdr, void * data); private: ImageBase(); // no default constructor ImageBase(const ImageBase &); // no copy constructor - protected: + protected: virtual int ProcessFrame() = 0; // Main functions for device thread. - virtual void Main(); + virtual void Main(); virtual int MainSetup(); - + virtual void MainQuit(); + // Input camera stuff Device *camera_driver; player_devaddr_t camera_addr; Modified: code/player/trunk/server/drivers/blobfinder/simpleshape/simpleshape.cc =================================================================== --- code/player/trunk/server/drivers/blobfinder/simpleshape/simpleshape.cc 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/blobfinder/simpleshape/simpleshape.cc 2009-01-24 23:14:21 UTC (rev 7297) @@ -146,8 +146,8 @@ SimpleShape( ConfigFile* cf, int section); // Setup/shutdown routines. - virtual int Setup(); - virtual int Shutdown(); + virtual int MainSetup(); + virtual void MainQuit(); // Load a shape model private: int LoadModel(); @@ -261,7 +261,7 @@ //////////////////////////////////////////////////////////////////////////////// // Set up the device (called by server thread). -int SimpleShape::Setup() +int SimpleShape::MainSetup() { // Load the shape model if (this->LoadModel() != 0) @@ -275,17 +275,15 @@ //////////////////////////////////////////////////////////////////////////////// // Shutdown the device (called by server thread). -int SimpleShape::Shutdown() +void SimpleShape::MainQuit() { - ImageBase::Shutdown(); + ImageBase::MainQuit(); // Free images if (this->inpImage) cvReleaseImage(&(this->inpImage)); if (this->outImage) cvReleaseImage(&(this->outImage)); - - return 0; } Modified: code/player/trunk/server/drivers/blobfinder/upcbarcode/upcbarcode.cc =================================================================== --- code/player/trunk/server/drivers/blobfinder/upcbarcode/upcbarcode.cc 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/blobfinder/upcbarcode/upcbarcode.cc 2009-01-24 23:14:21 UTC (rev 7297) @@ -129,8 +129,8 @@ public: UPCBarcode( ConfigFile* cf, int section); // Setup/shutdown routines. - virtual int Setup(); - virtual int Shutdown(); + virtual int MainSetup(); + virtual void MainQuit(); // Look for barcodes in the image. private: int ProcessFrame(); @@ -218,24 +218,22 @@ //////////////////////////////////////////////////////////////////////////////// // Setup the device (called by server thread). -int UPCBarcode::Setup() +int UPCBarcode::MainSetup() { - return ImageBase::Setup(); + return ImageBase::MainSetup(); } //////////////////////////////////////////////////////////////////////////////// // Shutdown the device (called by server thread). -int UPCBarcode::Shutdown() +int UPCBarcode::MainQuit() { - ImageBase::Shutdown(); + ImageBase::MainQuit(); if (this->inpImage) cvReleaseImage(&(this->inpImage)); if (this->outImage) cvReleaseImage(&(this->outImage)); inpImage = outImage = NULL; - - return 0; } Modified: code/player/trunk/server/drivers/camera/sphere/sphere_mixed.cc =================================================================== --- code/player/trunk/server/drivers/camera/sphere/sphere_mixed.cc 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/camera/sphere/sphere_mixed.cc 2009-01-24 23:14:21 UTC (rev 7297) @@ -359,13 +359,10 @@ //////////////////////////////////////////////////////////////////////////////// // Shutdown the device -int SphereDriver::Shutdown() +void SphereDriver::MainQuit() { puts("Shutting Sphere driver down"); - // Stop and join the driver thread - StopThread(); - // Set to 0,0 set_pan_and_tilt(mFg->fd, 0, 0); @@ -377,12 +374,7 @@ if (mFg != NULL) fg_close(mFg); - // Here you would shut the device down by, for example, closing a - // serial port. - puts("Sphere driver has been shutdown"); - - return(0); } //////////////////////////////////////////////////////////////////////////////// Modified: code/player/trunk/server/drivers/camera/sphere/sphere_mixed.h =================================================================== --- code/player/trunk/server/drivers/camera/sphere/sphere_mixed.h 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/camera/sphere/sphere_mixed.h 2009-01-24 23:14:21 UTC (rev 7297) @@ -43,7 +43,7 @@ // Must implement the following methods. int MainSetup(); - int Shutdown(); + void MainQuit(); // Main function for device thread. virtual void Main(); Modified: code/player/trunk/server/drivers/camera/yarp/YarpImage.cc =================================================================== --- code/player/trunk/server/drivers/camera/yarp/YarpImage.cc 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/camera/yarp/YarpImage.cc 2009-01-24 23:14:21 UTC (rev 7297) @@ -110,7 +110,7 @@ // Implementations of virtual functions virtual int MainSetup (); - virtual int Shutdown (); + virtual void MainQuit (); // Camera interface (provides) player_devaddr_t cam_id; @@ -213,11 +213,8 @@ //////////////////////////////////////////////////////////////////////////////// // Shutdown the device -int Yarp_Image::Shutdown () +void Yarp_Image::MainQuit () { - // Stop the driver thread - StopThread (); - // Disconnect the two ports Network::disconnect (imagePortName, portName); @@ -225,7 +222,6 @@ portIn.close (); PLAYER_MSG0 (1, "> Yarp_Image driver shutting down... [done]"); - return (0); } //////////////////////////////////////////////////////////////////////////////// Modified: code/player/trunk/server/drivers/joystick/linuxjoy.cc =================================================================== --- code/player/trunk/server/drivers/joystick/linuxjoy.cc 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/joystick/linuxjoy.cc 2009-01-24 23:14:21 UTC (rev 7297) @@ -252,7 +252,7 @@ // Must implement the following methods. public: int MainSetup(); - public: int Shutdown(); + public: void MainQuit(); // Main function for device thread. private: virtual void Main(); @@ -460,11 +460,8 @@ //////////////////////////////////////////////////////////////////////////////// // Shutdown the device -int LinuxJoystick::Shutdown() +void LinuxJoystick::MainQuit() { - // Stop and join the driver thread - this->StopThread(); - if ((this->cmd_position_addr.interf) && (this->position)) this->position->Unsubscribe(this->InQueue); if ((this->cmd_gripper_addr.interf) && (this->gripper)) @@ -472,8 +469,6 @@ // Close the joystick close(this->fd); - - return(0); } Modified: code/player/trunk/server/drivers/laser/RS4LeuzeLaserDriver.cc =================================================================== --- code/player/trunk/server/drivers/laser/RS4LeuzeLaserDriver.cc 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/laser/RS4LeuzeLaserDriver.cc 2009-01-24 23:14:21 UTC (rev 7297) @@ -157,7 +157,7 @@ //The driver must implement the abstract Setup and Shutdown methods (3); // Implementations of virtual functions int MainSetup(); - int Shutdown(); + void MainQuit(); //The drivers re-implements the ProcessMessage method to provide support for //handling request and commands(4) // This method will be invoked on each incoming message @@ -321,13 +321,8 @@ //////////////////////////////////////////////////////////////////////////////// // Shutdown the device -int RS4LeuzeLaserDriver::Shutdown() { - // Stop and join the driver thread - StopThread(); - - myLaser->closeSerial(); - - return(0); +void RS4LeuzeLaserDriver::MainQuit() { + myLaser->closeSerial(); } Modified: code/player/trunk/server/drivers/laser/bumper2laser.cc =================================================================== --- code/player/trunk/server/drivers/laser/bumper2laser.cc 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/laser/bumper2laser.cc 2009-01-24 23:14:21 UTC (rev 7297) @@ -101,7 +101,7 @@ // Must implement the following methods. virtual int MainSetup(); - virtual int Shutdown(); + virtual void MainQuit(); // This method will be invoked on each incoming message virtual int ProcessMessage(QueuePointer & resp_queue, @@ -246,15 +246,10 @@ //////////////////////////////////////////////////////////////////////////////// // Shutdown the device -int Bumper2Laser::Shutdown() +void Bumper2Laser::MainQuit() { - // Stop and join the driver thread - StopThread(); - // Unsubscribe from the bumper this->bumper_dev->Unsubscribe(this->InQueue); - - return 0; } //////////////////////////////////////////////////////////////////////////////// Modified: code/player/trunk/server/drivers/laser/sicklms200.cc =================================================================== --- code/player/trunk/server/drivers/laser/sicklms200.cc 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/laser/sicklms200.cc 2009-01-24 23:14:21 UTC (rev 7297) @@ -229,7 +229,7 @@ SickLMS200(ConfigFile* cf, int section); int MainSetup(); - int Shutdown(); + void MainQuit(); // MessageHandler int ProcessMessage(QueuePointer & resp_queue, @@ -609,11 +609,8 @@ //////////////////////////////////////////////////////////////////////////////// // Shutdown the device -int SickLMS200::Shutdown() +void SickLMS200::MainQuit() { - // shutdown laser device - StopThread(); - // switch to connect rate just in case if (this->connect_rate != this->current_rate) // PBeeson -- This doesn't do anything. Once Setup() completes, @@ -627,8 +624,6 @@ PLAYER_MSG0(2, "laser shutdown"); - - return(0); } Modified: code/player/trunk/server/drivers/localization/amcl/amcl_laser.cc =================================================================== --- code/player/trunk/server/drivers/localization/amcl/amcl_laser.cc 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/localization/amcl/amcl_laser.cc 2009-01-24 23:14:21 UTC (rev 7297) @@ -109,7 +109,7 @@ if(!(msg = laser_dev->Request(AMCL.InQueue, PLAYER_MSGTYPE_REQ, PLAYER_LASER_REQ_GET_GEOM, - NULL, 0, NULL,false))) + NULL, 0, NULL))) { PLAYER_WARN("failed to get laser geometry"); this->laser_pose.v[0] = 0.0; @@ -163,7 +163,7 @@ if(!(msg = mapdev->Request(AMCL.InQueue, PLAYER_MSGTYPE_REQ, PLAYER_MAP_REQ_GET_INFO, - NULL, 0, NULL, false))) + NULL, 0, NULL))) { PLAYER_ERROR("failed to get map info"); return(-1); @@ -215,7 +215,7 @@ if(!(msg = mapdev->Request(AMCL.InQueue, PLAYER_MSGTYPE_REQ, PLAYER_MAP_REQ_GET_DATA, - (void*)data_req,0,NULL,false))) + (void*)data_req,0,NULL))) { PLAYER_ERROR("failed to get map info"); free(data_req); Modified: code/player/trunk/server/drivers/mixed/chatterbox/cb_driver.cc =================================================================== --- code/player/trunk/server/drivers/mixed/chatterbox/cb_driver.cc 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/mixed/chatterbox/cb_driver.cc 2009-01-24 23:14:21 UTC (rev 7297) @@ -53,7 +53,7 @@ // Must implement the following methods. virtual int MainSetup(); - virtual int Shutdown(); + virtual void MainQuit(); virtual int ProcessMessage(QueuePointer & resp_queue, player_msghdr * hdr, void * data); @@ -273,7 +273,7 @@ //////////////////////////////////////////////////////////////////////////////// // Shutdown the device -int ChatterboxDriver::Shutdown() +void ChatterboxDriver::MainQuit() { puts("Shutting down Chatterbox driver..."); @@ -281,12 +281,7 @@ for( unsigned int l=0; l<LEDCOUNT; l++ ) setLed( l, 0,0,0 ); - // Stop and join the driver thread - this->StopThread(); - puts("done."); - - return(0); } Modified: code/player/trunk/server/drivers/mixed/cmucam2/cmucam2.cc =================================================================== --- code/player/trunk/server/drivers/mixed/cmucam2/cmucam2.cc 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/mixed/cmucam2/cmucam2.cc 2009-01-24 23:14:21 UTC (rev 7297) @@ -170,7 +170,7 @@ virtual void Main(); int MainSetup(); - int Shutdown(); + void MainQuit(); }; //////////////////////////////////////////////////////////////////////////////// @@ -292,12 +292,10 @@ //////////////////////////////////////////////////////////////////////////////// -int Cmucam2::Shutdown() +void Cmucam2::MainQuit() { - StopThread(); stop_tracking(fd); close_port(fd); // close the serial port - return(0); } //////////////////////////////////////////////////////////////////////////////// Modified: code/player/trunk/server/drivers/mixed/erratic/erratic.cc =================================================================== --- code/player/trunk/server/drivers/mixed/erratic/erratic.cc 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/mixed/erratic/erratic.cc 2009-01-24 23:14:21 UTC (rev 7297) @@ -215,7 +215,7 @@ // Constructor of the driver from configuration entry Erratic::Erratic(ConfigFile* cf, int section) - : ThreadedDriver(cf,section,true,PLAYER_MSGQUEUE_DEFAULT_MAXLEN) + : Driver(cf,section,true,PLAYER_MSGQUEUE_DEFAULT_MAXLEN) { // zero ids, so that we'll know later which interfaces were requested memset(&this->position_id, 0, sizeof(player_devaddr_t)); Modified: code/player/trunk/server/drivers/mixed/erratic/erratic.h =================================================================== --- code/player/trunk/server/drivers/mixed/erratic/erratic.h 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/mixed/erratic/erratic.h 2009-01-24 23:14:21 UTC (rev 7297) @@ -3,7 +3,7 @@ /** * Copyright (C) 2006 * Videre Design - * Copyright (C) 2000 + * Copyright (C) 2000 * Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard * * Videre Erratic robot driver for Player @@ -94,7 +94,7 @@ set_pid_rot_p = 83, set_pid_rot_v = 84, set_pid_rot_i = 85, - + } command_e; // Argument types used in robot commands @@ -135,7 +135,7 @@ class ErraticMotorPacket; -class Erratic : public ThreadedDriver +class Erratic : public Driver { private: int mcount; @@ -157,10 +157,10 @@ //ErraticMotorPacket* sippacket; ErraticMotorPacket *motor_packet; pthread_mutex_t motor_packet_mutex; - + int Connect(); int Disconnect(); - + void ResetRawPositions(); void ToggleMotorPower(unsigned char val); @@ -179,13 +179,13 @@ void PublishAIn(); void PublishIR(); void PublishSonar(); - + float IRRangeFromVoltage(float voltage); float IRFloorRange(float value); - + void StartThreads(); void StopThreads(); - + void Send(ErraticPacket *packet); void SendThread(); static void *SendThreadDummy(void *driver); @@ -211,11 +211,11 @@ bool print_all_packets; bool print_status_summary; - + bool save_settings_in_robot; int param_idx; // index in the RobotParams table for this robot - + // Max motor speeds (mm/sec,deg/sec) int motor_max_speed; int motor_max_turnspeed; @@ -228,7 +228,7 @@ uint16_t motor_pwm_frequency, motor_pwm_max_on; // Bound the command velocities - bool use_vel_band; + bool use_vel_band; // Max motor accel/decel (mm/sec/sec, deg/sec/sec) short motor_max_trans_accel, motor_max_trans_decel; Modified: code/player/trunk/server/drivers/mixed/evolution/er1/er.cc =================================================================== --- code/player/trunk/server/drivers/mixed/evolution/er1/er.cc 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/mixed/evolution/er1/er.cc 2009-01-24 23:14:21 UTC (rev 7297) @@ -488,14 +488,12 @@ er->Main(); } -int - ER::Shutdown() +void + ER::MainQuit() { if(this->_fd == -1) - return(0); + return; - StopThread(); - // the robot is stopped by the thread cleanup function StopRobot(), which // is called as a result of the above StopThread() @@ -510,8 +508,6 @@ this->_fd = -1; if( _debug ) printf("ER has been shutdown\n\n"); - - return(0); } void Modified: code/player/trunk/server/drivers/mixed/evolution/er1/er.h =================================================================== --- code/player/trunk/server/drivers/mixed/evolution/er1/er.h 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/mixed/evolution/er1/er.h 2009-01-24 23:14:21 UTC (rev 7297) @@ -2,8 +2,8 @@ * Player - One Hell of a Robot Server * Copyright (C) 2000-2003 * David Feil-Seifer - * * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -22,7 +22,7 @@ /* * - * Relevant constants for the "ER" robots, made by Evolution Robotics. + * Relevant constants for the "ER" robots, made by Evolution Robotics. * * Some of this code is borrowed and/or adapted from the player * module of trogdor; thanks to the author of that module. @@ -90,11 +90,11 @@ -class ER : public ThreadedDriver +class ER : public ThreadedDriver { private: player_er1_data_t er1_data; - + player_devaddr_t position_id; int position_subscriptions; @@ -106,29 +106,25 @@ void Stop( int StopMode ); virtual void Main(); virtual int MainSetup(); - virtual int Shutdown(); - //void HandleConfig(void); - //void GetCommand(void); - //void PutData(void); - + virtual void MainQuit(); + // MessageHandler virtual int ProcessMessage(QueuePointer & resp_queue, player_msghdr * hdr, void * data); - //void HandlePositionCommand(player_position2d_cmd_t position_cmd); void Test(); - + private: // this function will be run in a separate thread int InitOdom(); int InitRobot(); - + //serial connection int *_tc_num; int _fd; // device file descriptor const char* _serial_port; // name of dev file bool _fd_blocking; - + // methods for internal use int WriteBuf(unsigned char* s, size_t len); int ReadBuf(unsigned char* s, size_t len); @@ -162,7 +158,7 @@ int _last_ltics, _last_rtics; double _px, _py, _pa; // integrated odometric pose (m,m,rad) int _powered, _resting; // If _powered is false, no constant updates. _resting means the last update was a 0,0 one. - + int send_command( unsigned char address, unsigned char c, int ret_num, unsigned char * ret ); int send_command_2_arg( unsigned char address, unsigned char c, int arg, int ret_num, unsigned char * ret ); int send_command_4_arg( unsigned char address, unsigned char c, int arg, int ret_num, unsigned char * ret ); Modified: code/player/trunk/server/drivers/mixed/garcia/garcia_mixed.cc =================================================================== --- code/player/trunk/server/drivers/mixed/garcia/garcia_mixed.cc 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/mixed/garcia/garcia_mixed.cc 2009-01-24 23:14:21 UTC (rev 7297) @@ -304,22 +304,14 @@ //////////////////////////////////////////////////////////////////////////////// // Shutdown the device -int -GarciaDriver::Shutdown() +void +GarciaDriver::MainQuit() { puts("Shutting Garcia driver down"); - // Stop and join the driver thread - StopThread(); - - // Here you would shut the device down by, for example, closing a - // serial port. - delete mGarcia; puts("Garcia driver has been shutdown"); - - return(0); } //////////////////////////////////////////////////////////////////////////////// Modified: code/player/trunk/server/drivers/mixed/garcia/garcia_mixed.h =================================================================== --- code/player/trunk/server/drivers/mixed/garcia/garcia_mixed.h 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/mixed/garcia/garcia_mixed.h 2009-01-24 23:14:21 UTC (rev 7297) @@ -39,7 +39,7 @@ // Must implement the following methods. int MainSetup(); - int Shutdown(); + void MainQuit(); // Main function for device thread. virtual void Main(); Modified: code/player/trunk/server/drivers/mixed/irobot/create/create_driver.cc =================================================================== --- code/player/trunk/server/drivers/mixed/irobot/create/create_driver.cc 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/mixed/irobot/create/create_driver.cc 2009-01-24 23:14:21 UTC (rev 7297) @@ -120,7 +120,7 @@ ~Create(); int MainSetup(); - int Shutdown(); + void MainQuit(); // MessageHandler int ProcessMessage(QueuePointer &resp_queue, @@ -267,18 +267,15 @@ return(0); } -int -Create::Shutdown() +void +Create::MainQuit() { - this->StopThread(); - if(create_close(this->create_dev)) { PLAYER_ERROR("failed to close create connection"); } create_destroy(this->create_dev); this->create_dev = NULL; - return(0); } void Modified: code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_driver.cc =================================================================== --- code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_driver.cc 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_driver.cc 2009-01-24 23:14:21 UTC (rev 7297) @@ -123,7 +123,7 @@ Roomba(ConfigFile* cf, int section); int MainSetup(); - int Shutdown(); + void MainQuit(); // MessageHandler int ProcessMessage(QueuePointer & resp_queue, @@ -276,18 +276,15 @@ return(0); } -int -Roomba::Shutdown() +void +Roomba::MainQuit() { - this->StopThread(); - if(roomba_close(this->roomba_dev)) { PLAYER_ERROR("failed to close roomba connection"); } roomba_destroy(this->roomba_dev); this->roomba_dev = NULL; - return(0); } void Modified: code/player/trunk/server/drivers/mixed/mricp/src/mricp_driver.cpp =================================================================== --- code/player/trunk/server/drivers/mixed/mricp/src/mricp_driver.cpp 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/mixed/mricp/src/mricp_driver.cpp 2009-01-24 23:14:21 UTC (rev 7297) @@ -252,7 +252,7 @@ // Must implement the following methods. public : virtual int MainSetup(); - virtual int Shutdown(); + virtual void MainQuit(); virtual int ProcessMessage(QueuePointer& resp_queue, player_msghdr * hdr, void * data); // Constructor public: MrIcp(ConfigFile* cf, int section); @@ -587,7 +587,7 @@ fprintf(config_file,"%s %.3f %.3f %.3f\n",filename,global_pose.p.x,global_pose.p.y,global_pose.phi); this->global_pose.p.x = this->global_pose.p.y = this->global_pose.phi = 0; } -int MrIcp::Shutdown() +void MrIcp::MainQuit() { // Stop and join the driver thread cout<<"\n- Shutting Down MRICP Driver - Cleaning up Mess ..\n"; fflush(stdout); @@ -613,10 +613,9 @@ fclose(config_file); cout<<" Files Closed ->"; fflush(stdout); //ConnectPatches(); - this->StopThread(); cout<<" Thread Killed ->"; fflush(stdout); cout<<" ... ShutDown FINISED\n"; fflush(stdout); - return(0); + }; // this function will run in a separate thread void MrIcp::Main() Modified: code/player/trunk/server/drivers/mixed/p2os/p2os.cc =================================================================== --- code/player/trunk/server/drivers/mixed/p2os/p2os.cc 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/mixed/p2os/p2os.cc 2009-01-24 23:14:21 UTC (rev 7297) @@ -1291,7 +1291,7 @@ return(0); } -int P2OS::Shutdown() +void P2OS::MainQuit() { unsigned char command[20],buffer[20]; P2OSPacket packet; @@ -1299,10 +1299,8 @@ memset(buffer,0,20); if(this->psos_fd == -1) - return(0); + return; - this->StopThread(); - command[0] = STOP; packet.Build(command, 1); packet.Send(this->psos_fd); @@ -1318,8 +1316,6 @@ puts("P2OS has been shutdown"); delete this->sippacket; this->sippacket = NULL; - - return(0); } P2OS::~P2OS (void) Modified: code/player/trunk/server/drivers/mixed/p2os/p2os.h =================================================================== --- code/player/trunk/server/drivers/mixed/p2os/p2os.h 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/mixed/p2os/p2os.h 2009-01-24 23:14:21 UTC (rev 7297) @@ -330,7 +330,7 @@ virtual void Main(); virtual int MainSetup(); - virtual int Shutdown(); + virtual void MainQuit(); // MessageHandler virtual int ProcessMessage(QueuePointer & resp_queue, Modified: code/player/trunk/server/drivers/mixed/phidgetIFK/phidgetIFK.cc =================================================================== --- code/player/trunk/server/drivers/mixed/phidgetIFK/phidgetIFK.cc 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/mixed/phidgetIFK/phidgetIFK.cc 2009-01-24 23:14:21 UTC (rev 7297) @@ -121,10 +121,10 @@ // Destructor ~PhidgetIFK(); - + Shutdown // Must implement the following methods. virtual int MainSetup(); - virtual int Shutdown(); + virtual void MainQuit(); // This method will be invoked on each incoming message virtual int ProcessMessage(QueuePointer &resp_queue, @@ -288,12 +288,9 @@ //////////////////////////////////////////////////////////////////////////////// // Shutdown the device -int PhidgetIFK::Shutdown() { +void PhidgetIFK::MainQuit() { PLAYER_MSG0(1,"Shutting PhidgetIFK driver down"); - // Stop and join the driver thread - StopThread(); - usleep(100000); CPhidget_close((CPhidgetHandle)ifk); CPhidget_delete((CPhidgetHandle)ifk); @@ -308,8 +305,6 @@ PLAYER_MSG0(1,"PhidgetIFK driver has been shutdown"); - - return(0); } int PhidgetIFK::ProcessMessage(QueuePointer &resp_queue, Modified: code/player/trunk/server/drivers/mixed/reb/reb.cc =================================================================== --- code/player/trunk/server/drivers/mixed/reb/reb.cc 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/mixed/reb/reb.cc 2009-01-24 23:14:21 UTC (rev 7297) @@ -345,13 +345,11 @@ } -int -REB::Shutdown() +void +REB::MainQuit() { printf("REB: SHUTDOWN\n"); - StopThread(); - SetSpeed(REB_MOTOR_LEFT, 0); SetSpeed(REB_MOTOR_RIGHT, 0); @@ -359,7 +357,6 @@ close(reb_fd); reb_fd = -1; - return(0); } int Modified: code/player/trunk/server/drivers/mixed/reb/reb.h =================================================================== --- code/player/trunk/server/drivers/mixed/reb/reb.h 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/mixed/reb/reb.h 2009-01-24 23:14:21 UTC (rev 7297) @@ -1,9 +1,9 @@ /* * Player - One Hell of a Robot Server - * Copyright (C) 2000 + * Copyright (C) 2000 * Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard - * * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -32,7 +32,7 @@ * power services all need to go through a single serial port and * base device class. So this code was copied from p2osdevice and * modified to taste. - * + * */ #ifndef _REBDEVICE_H @@ -97,30 +97,30 @@ */ -class REB : public ThreadedDriver +class REB : public ThreadedDriver { public: - + REB(ConfigFile *cf, int section); /* the main thread */ virtual void Main(); int ProcessMessage(ClientData * client, player_msghdr * hdr, uint8_t * data, uint8_t * resp_data, size_t * resp_len); - + // we override these, because we will maintain our own subscription count virtual int Subscribe(player_device_id_t id); virtual int Unsubscribe(player_device_id_t id); - + virtual int MainSetup(); - virtual int Shutdown(); + virtual void MainQuit(); void Restart(); void ReadConfig(); void SetOdometry(int, int, short); - + // handle IR void SetIRState(int); @@ -141,10 +141,10 @@ int ReadSpeed(int); void SetPos(int, int); - + void SetPosCounter(int, int); int ReadPos(int); - + unsigned char ReadStatus(int, int *, int *); void ConfigPosPID(int, int, int, int); void ConfigSpeedPID(int, int, int, int); @@ -159,13 +159,13 @@ player_device_id_t ir_id; player_device_id_t position_id; player_device_id_t power_id; - + int ir_subscriptions; int position_subscriptions; - + int param_index; // index in the RobotParams table for this robot int reb_fd; // reb device file descriptor - + struct timeval last_position; // last position update bool refresh_last_position; int last_lpos, last_rpos; @@ -189,10 +189,10 @@ bool direct_velocity_control; // device used to communicate with reb - char reb_serial_port[MAX_FILENAME_SIZE]; + char reb_serial_port[MAX_FILENAME_SIZE]; struct pollfd write_pfd, read_pfd; - + // holding vars for command processing int ProcessCommand(player_position_cmd_t * poscmd); short last_trans_command, last_rot_command; Modified: code/player/trunk/server/drivers/mixed/rflex/rflex.cc =================================================================== --- code/player/trunk/server/drivers/mixed/rflex/rflex.cc 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/mixed/rflex/rflex.cc 2009-01-24 23:14:21 UTC (rev 7297) @@ -793,20 +793,19 @@ { } -int RFLEX::Shutdown() +void RFLEX::MainQuit() { if(rflex_fd == -1) { - return 0; + return; } - StopThread(); //make sure it doesn't go anywhere rflex_stop_robot(rflex_fd,(int) M2ARB_ODO_CONV(rflex_configs.mPsec2_trans_acceleration)); //kill that infernal clicking rflex_sonars_off(rflex_fd); // release the port rflex_close_connection(&rflex_fd); - return 0; + return; } int Modified: code/player/trunk/server/drivers/mixed/rflex/rflex.h =================================================================== --- code/player/trunk/server/drivers/mixed/rflex/rflex.h 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/mixed/rflex/rflex.h 2009-01-24 23:14:21 UTC (rev 7297) @@ -1,9 +1,9 @@ /* * Player - One Hell of a Robot Server - * Copyright (C) 2000 + * Copyright (C) 2000 * Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard - * * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -36,7 +36,7 @@ #include <sys/time.h> #include <libplayercore/playercore.h> - + #include "rflex_commands.h" #include "rflex-io.h" @@ -56,7 +56,7 @@ #define RFLEX_BUMPER_STYLE_ADDR "addr" #define DEFAULT_RFLEX_BUMPER_STYLE RFLEX_BUMPER_STYLE_ADDR -enum +enum { BUMPER_BIT, BUMPER_ADDR @@ -93,7 +93,7 @@ // this is here because we need the above typedef's before including it. -class RFLEX : public ThreadedDriver +class RFLEX : public ThreadedDriver { private: player_devaddr_t position_id; @@ -113,9 +113,9 @@ int bumper_subscriptions; int rflex_fd; // rflex device file descriptor - + // device used to communicate with rflex - char rflex_serial_port[MAX_FILENAME_SIZE]; + char rflex_serial_port[MAX_FILENAME_SIZE]; double m_odo_x; double m_odo_y; double rad_odo_theta; @@ -139,12 +139,12 @@ virtual int Subscribe(player_devaddr_t addr); virtual int Unsubscribe(player_devaddr_t addr); - virtual int Shutdown(); + virtual void MainQuit(); static int joy_control; - + // MessageHandler - int ProcessMessage(QueuePointer & resp_queue, player_msghdr * hdr, + int ProcessMessage(QueuePointer & resp_queue, player_msghdr * hdr, void * data); }; Modified: code/player/trunk/server/drivers/mixed/rmp/segwayrmp.cc =================================================================== --- code/player/trunk/server/drivers/mixed/rmp/segwayrmp.cc 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/mixed/rmp/segwayrmp.cc 2009-01-24 23:14:21 UTC (rev 7297) @@ -267,22 +267,12 @@ return(0); } -int -SegwayRMP::Shutdown() +void +SegwayRMP::MainQuit() { PLAYER_MSG0(2, "Shutting down CAN bus"); fflush(stdout); - // TODO: segfaulting in here somewhere on client disconnect, but only - // sometimes. - // - // UPDATE: This might have been fixed by moving the call to StopThread() - // to before the sending of zero velocities. There could have been - // a race condition, since Shutdown() is called from the server's thread - // context. - - StopThread(); - // send zero velocities, for some semblance of safety CanPacket pkt; @@ -293,8 +283,6 @@ canio->Shutdown(); delete canio; canio = NULL; - - return(0); } // Main function for device thread. Modified: code/player/trunk/server/drivers/mixed/rmp/segwayrmp.h =================================================================== --- code/player/trunk/server/drivers/mixed/rmp/segwayrmp.h 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/mixed/rmp/segwayrmp.h 2009-01-24 23:14:21 UTC (rev 7297) @@ -30,14 +30,14 @@ // Driver for robotic Segway class SegwayRMP : public ThreadedDriver { - public: + public: // Constructors etc SegwayRMP(ConfigFile* cf, int section); ~SegwayRMP(); // Setup/shutdown routines. virtual int MainSetup(); - virtual int Shutdown(); + virtual void MainQuit(); virtual int ProcessMessage(QueuePointer & resp_queue, player_msghdr * hdr, void * data); @@ -50,7 +50,7 @@ player_devaddr_t position3d_id; player_position3d_data_t position3d_data; - + player_devaddr_t power_id; player_power_data_t power_data; @@ -90,7 +90,7 @@ // helper to read a cycle of data from the RMP int Read(); - + // Calculate the difference between two raw counter values, taking care // of rollover. int Diff(uint32_t from, uint32_t to, bool first); @@ -106,7 +106,7 @@ // helper to take a player command and turn it into a CAN command packet void MakeVelocityCommand(CanPacket* pkt, int32_t xspeed, int32_t yawspeed); - + void MakeShutdownCommand(CanPacket* pkt); void UpdateData(rmp_frame_t *); Modified: code/player/trunk/server/drivers/mixed/robotino/robotino_driver.cc =================================================================== --- code/player/trunk/server/drivers/mixed/robotino/robotino_driver.cc 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/mixed/robotino/robotino_driver.cc 2009-01-24 23:14:21 UTC (rev 7297) @@ -1,9 +1,9 @@ /*! \mainpage * Player - One Hell of a Robot Server - * Copyright (C) 2003 + * Copyright (C) 2003 * Brian Gerkey - * - * + * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -22,16 +22,16 @@ /*! \class RobotinoDriver * \brief Class for FESTO Robotino(R) plug-in driver - * \par Authors: + * \par Authors: * Simon Kracht and Carsten Nielsen * Aalborg University, Section for Automation and Control * \par Year: * 2007 * - * Plug-in driver for the FESTO Robotino(R) robot + * Plug-in driver for the FESTO Robotino(R) robot * * This driver makes use of the open C++ libraries bundled with - * the FESTO Robotino(R) robot, thus providing access to most + * the FESTO Robotino(R) robot, thus providing access to most * of the robots functionalities. * * \par Compile-time dependencies: @@ -68,7 +68,7 @@ * - PLAYER_IR_REQ_POSE * \b interface_power : * - PLAYER_POSITION2D_REQ_MOTOR_POWER - * + * * \par Configuration file example: * * \code @@ -76,7 +76,7 @@ * ( * name "robotino_driver" * provides ["position2d:0" "power:0" "bumper:0" "ir:0"] - * + * * # Options * ROBOTINO_TIMEOUT_MS 5000 #default 5000 * CYCLE_TIME_US 100000 #default 100000 @@ -137,38 +137,38 @@ class RobotinoDriver:public ThreadedDriver { public: - + // Constructor; need that RobotinoDriver (ConfigFile * cf, int section); - + // Must implement the following methods. virtual int MainSetup (); - virtual int Shutdown (); - + virtual void MainQuit (); + // This method will be invoked on each incoming message virtual int ProcessMessage (QueuePointer &resp_queue, player_msghdr * hdr, void *data); - // Holders for desired velocities + // Holders for desired velocities float desiredVelocityX; float desiredVelocityY; float desiredVelocityA; - + // Holder for position (odometry) data sent from below player_position2d_data_t posdata; - // Holder for position (odometry) data sent from above + // Holder for position (odometry) data sent from above player_position2d_set_odom_req odomCommand; private: // Main function for device thread. virtual void Main (); - + player_devaddr_t position_addr; player_devaddr_t power_addr; player_devaddr_t bumper_addr; player_devaddr_t ir_addr; - + // Robotino(R) device RobotinoCom com; @@ -178,12 +178,12 @@ }; /// A factory creation function. -/** +/** Declared outside of the class so that it can be invoked without any object context (alternatively, you can declare it static in the class). In this function, we create and return (as a generic Driver*) a pointer to a new instance of this driver. */ -Driver * +Driver * RobotinoDriver_Init (ConfigFile * cf, int section) { //! Create and return a new instance of this driver @@ -191,11 +191,11 @@ } //! A driver registration function. -/** - Again declared outside of the class so that it can be invoked without - object context. In this function, we add the driver into the given - driver table, indicating which interface the driver can support and how - to create a driver instance. +/** + Again declared outside of the class so that it can be invoked without + object context. In this function, we add the driver into the given + driver table, indicating which interface the driver can support and how + to create a driver instance. */ void RobotinoDriver_Register (DriverTable * table) { @@ -215,33 +215,33 @@ memset (&this->power_addr, 0, sizeof (player_devaddr_t)); memset (&this->bumper_addr, 0, sizeof (player_devaddr_t)); memset (&this->ir_addr, 0, sizeof(player_devaddr_t)); - + // Do we create a position interface? if (cf->ReadDeviceAddr (& (this-> position_addr),section,"provides",PLAYER_POSITION2D_CODE,-1, NULL) == 0) { if (this->AddInterface (this->position_addr) !=0) { - this->SetError (-1); + this->SetError (-1); return; } } - + // Do we create a power interface? if (cf->ReadDeviceAddr (&(this->power_addr),section,"provides",PLAYER_POWER_CODE,-1, NULL) == 0) { if (this->AddInterface (this->power_addr) != 0) { - this->SetError (-1); + this->SetError (-1); return; } } - + // Do we create a bumper interface? if (cf->ReadDeviceAddr (&(this->bumper_addr),section,"provides",PLAYER_BUMPER_CODE, -1, NULL) == 0) { if (this->AddInterface (this->bumper_addr) != 0) { - this->SetError (-1); + this->SetError (-1); return; } } @@ -250,67 +250,67 @@ { if(this->AddInterface(this->ir_addr) != 0) { - this->SetError(-1); + this->SetError(-1); return; } } -} +} /// Set up the device. -/** - Return 0 if things go well, and -1 otherwise. +/** + Return 0 if things go well, and -1 otherwise. */ int RobotinoDriver::MainSetup () { std::cout << std:: endl << "Robotino(R) :: Driver initialising" << std::endl; - + //! Here you do whatever is necessary to setup the device, like open and configure a serial port. - + XTimer timer; if (false == com.init ()) { return -1; } - + std::cout << std::endl << "Robotino(R) :: Connecting..."; - + com.setErrorCallback (&errorCb,NULL); com.setConnectedCallback (&connectedCb, NULL); com.setConnectionClosedCallback(&connectionClosedCb, NULL); - + com.connectToHost ("127.0.0.1"); - + while (com.state () == RobotinoCom::ConnectingState) { std::cout << "."; XThread::msleep (200); } - - // Camera settings + + // Camera settings RobotinoCom::CameraParameters param = com.cameraParameters(); param.compression = RobotinoCom::HighCompression; param.resolution = RobotinoCom::QVGA; com.setCameraParameters( param ); - + PLAYER_MSG0(2,"Robotino : Setup done"); - + // If errors are encountered if (com.error () != RobotinoCom::NoError) { - Shutdown (); + MainQuit (); } - + // Set the robot timeout, in seconds. The robot will stop if no commands are sent before the timeout expires. com.setTimeout (this->ROBOTINO_TIMEOUT_MS); - std::cout << std::endl << "Robotino(R) :: Timeout set to " << this->ROBOTINO_TIMEOUT_MS; + std::cout << std::endl << "Robotino(R) :: Timeout set to " << this->ROBOTINO_TIMEOUT_MS; std::cout << std::endl << "Robotino(R) :: Driver initializing - DONE" << std::endl; if (com.receiveIoStatus() == false) { PLAYER_ERROR("Robotino(R) :: Failed to receive IO status in setup"); - } - + } + // Initialize the holders for desired velocities desiredVelocityX = 0; desiredVelocityY = 0; @@ -325,7 +325,7 @@ /// Shutdown the device -int RobotinoDriver::Shutdown () +void RobotinoDriver::MainQuit () { std::cout << std::endl << "Robotino(R) :: Shutting driver down"; //! Stop and join the driver thread @@ -335,9 +335,9 @@ //! serial port. com.close (); - + std::cout << std::endl << "Robotino(R) :: Shutting driver down - DONE" << std::endl; - + return (0); } @@ -348,7 +348,7 @@ //! Send a response if necessary, using Publish(). //! If you handle the message successfully, return 0. Otherwise, //! return -1, and a NACK will be sent for you, if a response is required. - + // Interface - position2d if (Message::MatchMessage (hdr,PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL, @@ -357,12 +357,12 @@ // Get and send the latest motor commands player_position2d_cmd_vel_t position_cmd; position_cmd = *(player_position2d_cmd_vel_t *)data; - + /*PLAYER_MSG3 (3,"Robotino(R) :: Player motor commands %f:%f:%f", position_cmd.vel.px, position_cmd.vel.py, position_cmd.vel.pa);*/ - + // Desired velocities are stored desiredVelocityX = position_cmd.vel.px; // [m/s] desiredVelocityY = position_cmd.vel.py; // [m/s] @@ -373,12 +373,12 @@ position_cmd.vel.pa = position_cmd.vel.pa * (180/PI); /* - + if (position_cmd.vel.px < 0) { position_cmd.vel.px = position_cmd.vel.px * 5; } - + if (position_cmd.vel.py < 0) { position_cmd.vel.py = position_cmd.vel.py * 5; @@ -388,7 +388,7 @@ com.setVelocity (position_cmd.vel.px, position_cmd.vel.py, position_cmd.vel.pa); - + // Sending the set values to Robotino and receiving the sensor readings if (com.receiveIoStatus() == false) { @@ -396,8 +396,8 @@ } return (0); } - - + + // Command from above to set the odometry to a particular value else if (Message::MatchMessage (hdr,PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_SET_ODOM, @@ -406,12 +406,12 @@ this->Publish(this->position_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_SET_ODOM); - + return (0); } - - - + + + else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_GET_GEOM, @@ -422,18 +422,18 @@ geom.size.sl = ROBOTINO_DIAMETER; geom.size.sw = ROBOTINO_DIAMETER; - - this->Publish (this->position_addr, + + this->Publish (this->position_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_GET_GEOM, (void *) &geom, sizeof (geom), NULL); - + return (0); } - + // Interface - bumper else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ, PLAYER_BUMPER_REQ_GET_GEOM, @@ -457,7 +457,7 @@ (void*)&geom, sizeof(geom), NULL); delete geom.bumper_def; - + return(0); } // Interface - ir @@ -469,13 +469,13 @@ memset(&geom, 0, sizeof(geom)); geom.poses_count = com.numDistanceSensors(); geom.poses = new player_pose3d_t [geom.poses_count]; - + for(unsigned int intCount = 0;intCount < geom.poses_count;intCount++) { geom.poses[intCount].pyaw = DTOR(40*intCount); geom.poses[intCount].px = cos(geom.poses[intCount].pyaw)*ROBOTINO_RADIUS; geom.poses[intCount].py = sin(geom.poses[intCount].pyaw)*ROBOTINO_RADIUS; - + } this->Publish(this->ir_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, @@ -486,7 +486,7 @@ delete [] geom.poses; return (0); } - + // Motor power request - NOTE: no action taken since motors are turned on all the time else if (Message::MatchMessage (hdr,PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_MOTOR_POWER, @@ -495,13 +495,13 @@ this->Publish(this->position_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_MOTOR_POWER); - + return (0); } - + else PLAYER_ERROR("Robotino:: Unhandled message"); - return (-1); + return (-1); } @@ -512,59 +512,59 @@ // float arrActualVelocities[intNumberOfMotors]; //! The main loop; interact with the device here - + XTimer timerEuler; - + player_ir_data_t irdata; memset (&irdata, 0,sizeof (irdata)); - + player_power_data_t powerdata; memset(&powerdata,0,sizeof(powerdata)); player_bumper_data_t bumperdata; memset(&bumperdata,0,sizeof(bumperdata)); - + // player_position2d_data_t posdata; // memset(&posdata,0,sizeof(posdata)); - + // Update position2d data float floatTheta = 0; float floatPhi1 = 60*(PI/180); float floatPhi2 = 180*(PI/180); float floatPhi3 = 300*(PI/180); - + // Matrix elements float floatA11= -sin(floatPhi1 + floatTheta); float floatA12= cos(floatPhi1 + floatTheta); float floatA13= ROBOTINO_CENTRE_TO_WHEEL; - + float floatA21 = -sin(floatPhi2 + floatTheta); float floatA22 = cos(floatPhi2 + floatTheta); float floatA23 = ROBOTINO_CENTRE_TO_WHEEL; - + float floatA31 = -sin(floatPhi3 + floatTheta); float floatA32 = cos(floatPhi3 + floatTheta); float floatA33 = ROBOTINO_CENTRE_TO_WHEEL; - + // Calculate determinant float floatInvDeterminant = 1/(floatA11*(floatA33*floatA22-floatA32*floatA23)-floatA21*(floatA33*floatA12-floatA32*floatA13)+floatA31*(floatA23*floatA12-floatA22*floatA13)); - - while (true) - + + while (true) + { // Test if we are supposed to cancel pthread_testcancel(); - + // Process incoming messages ProcessMessages(); - + //! Interact with the device, and push out the resulting data, using Driver::Publish() - - + + int intNumberOfMotors = 3; float arrActualVelocities[3]; int intCount = 0; - + // Get motor speeds for (intCount = 0; intCount <= intNumberOfMotors-1; intCount++) { @@ -573,12 +573,12 @@ intCount, arrActualVelocities[intCount]);*/ } - + // Calculate velocities posdata.vel.px = (PI/30)*ROBOTINO_WHEEL_RADIUS*floatInvDeterminant*((floatA33*floatA22-floatA32*floatA23)*arrActualVelocities[0]-(floatA33*floatA12-floatA32*floatA13)*arrActualVelocities[1]+(floatA23*floatA12-floatA22*floatA13)*arrActualVelocities[2]); posdata.vel.py = (PI/30)*ROBOTINO_WHEEL_RADIUS*floatInvDeterminant*(-(floatA33*floatA21-floatA31*floatA23)*arrActualVelocities[0]+(floatA33*floatA11-floatA31*floatA13)*arrActualVelocities[1]-(floatA23*floatA11-floatA21*floatA13)*arrActualVelocities[2]); posdata.vel.pa = (PI/30)*ROBOTINO_WHEEL_RADIUS*floatInvDeterminant*((floatA32*floatA21-floatA31*floatA22)*arrActualVelocities[0]-(floatA32*floatA11-floatA31*floatA12)*arrActualVelocities[1]+(floatA22*floatA11-floatA21*floatA12)*arrActualVelocities[2]); - + // Calculate positions posdata.pos.px += posdata.vel.px * timerEuler.msecsElapsed()/1000; posdata.pos.py += posdata.vel.py * timerEuler.msecsElapsed()/1000; @@ -589,43 +589,43 @@ { posdata.pos.pa += 2*PI; } - + else if(posdata.pos.pa >PI) { posdata.pos.pa -= 2*PI; } - - + + timerEuler.start(); - + this->Publish(this->position_addr, PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE, (void*)&posdata,sizeof(posdata), NULL); - + // Update power data powerdata.volts = com.voltageBatt1plus2(); powerdata.watts = com.voltageBatt1plus2() * com.current(); powerdata.valid = (PLAYER_POWER_MASK_VOLTS | PLAYER_POWER_MASK_WATTS); - + this->Publish(this->power_addr, PLAYER_MSGTYPE_DATA, PLAYER_POWER_DATA_STATE, (void*)&powerdata, sizeof(powerdata), NULL); - - + + // Update bumper data bumperdata.bumpers_count = 1; bumperdata.bumpers = new uint8_t; bumperdata.bumpers[0] = com.bumper(); - + //PLAYER_MSG1(2,"Robotino(R) :: Bumper = %d",bumperdata.bumpers[0]); - + this->Publish(this->bumper_addr, PLAYER_MSGTYPE_DATA, PLAYER_BUMPER_DATA_STATE, (void*)&bumperdata, sizeof(bumperdata), NULL); delete bumperdata.bumpers; - + // Update IR data // Number of IR sensors on Robotino(R) irdata.ranges_count = com.numDistanceSensors(); @@ -635,8 +635,8 @@ { //! Get measured distances from each sensor irdata.ranges[intCount] = com.distance(intCount+1); - - irdata.ranges[intCount] = (1024 - irdata.ranges[intCount])*0.001; + + irdata.ranges[intCount] = (1024 - irdata.ranges[intCount])*0.001; /*PLAYER_MSG2(2,"Robotino(R) :: IR sensor %d = %f", intCount+1, irdata.ranges[intCount]);*/ @@ -650,8 +650,8 @@ sizeof (irdata), NULL); delete [] irdata.ranges; - - // To maintain connection due to RobotinoCom timeout + + // To maintain connection due to RobotinoCom timeout if((desiredVelocityX == 0) && (desiredVelocityY == 0) && (desiredVelocityA == 0)) { if (com.receiveIoStatus() == false) @@ -659,9 +659,9 @@ PLAYER_ERROR("Robotino(R) :: Failed to receive IO status in setup"); } } - + // Sleep (you might, for example, block on a read() instead) usleep (this->CYCLE_TIME_US); } } - + Modified: code/player/trunk/server/drivers/mixed/sr3000/sr3000.cc =================================================================== --- code/player/trunk/server/drivers/mixed/sr3000/sr3000.cc 2009-01-24 04:43:40 UTC (rev 7296) +++ code/player/trunk/server/drivers/mixed/sr3000/sr3000.cc 2009-01-24 23:14:21 UTC (rev 7297) @@ -31,7 +31,7 @@ camera option parameters are supported, via the libusbSR library. The driver provides a @ref interface_pointcloud3d interface and two @ref interface_camera interfaces for both distance and intensity images, *or* a @ref interface_stereo -interface. +interface. This version of the driver works with libusbSR v1.0.10+ @@ -46,7 +46,7 @@ the SR3000 - @ref interface_stereo : intensity and distance images as left and right channels, and the 3d point cloud generated by the SR3000 - + @par Requires - none @@ -82,9 +82,9 @@ name "sr3000" provides ["pointcloud3d:0" "distance:::camera:0" "intensity:::camera:1"] - + # OR ... - + provides ["stereo:0"] ) @endverbatim @@ -111,7 +111,7 @@ ~SR3000 (); int MainSetup (); - int Shutdown (); + void MainQuit (); // MessageHandler virtual int ProcessMessage (QueuePointer &resp_queue, @@ -133,7 +133,7 @@ // SR3000 specific values unsigned int rows, cols, inr; - + ImgEntry* imgEntryArray; float *buffer, *xp, *yp, *zp; @@ -147,7 +147,7 @@ protected: // Properties IntProperty auto_illumination, integration_time, modulation_freq, amp_threshold; - + bool providePCloud, provideDCam, provideICam, provideStereo; }; @@ -183,7 +183,7 @@ memset (&this->pcloud_addr, 0, sizeof (player_devaddr_t)); memset (&this->d_cam_addr, 0, sizeof (player_devaddr_t)); memset (&this->i_cam_addr, 0, sizeof (player_devaddr_t)); - + this->RegisterProperty ("auto_illumination", &this->auto_illumination, cf, section); this->RegisterProperty ("integration_time", &this->integration_time, cf, section); this->RegisterProperty ("modulation_freq", &this->modulation_freq, cf, section); @@ -214,7 +214,7 @@ } providePCloud = TRUE; } - + // Outgoing distance::camera:0 interface i... [truncated message content] |
From: <th...@us...> - 2009-06-06 10:23:37
|
Revision: 7803 http://playerstage.svn.sourceforge.net/playerstage/?rev=7803&view=rev Author: thjc Date: 2009-06-06 10:23:35 +0000 (Sat, 06 Jun 2009) Log Message: ----------- applied patch 2666932 Allow Control of Canon VCC4 Camera through P2OS Driver fixed sign of var in writelog Modified Paths: -------------- code/player/trunk/server/drivers/mixed/p2os/p2os.cc code/player/trunk/server/drivers/mixed/p2os/p2os.h code/player/trunk/server/drivers/shell/writelog.cc Modified: code/player/trunk/server/drivers/mixed/p2os/p2os.cc =================================================================== --- code/player/trunk/server/drivers/mixed/p2os/p2os.cc 2009-06-06 09:51:04 UTC (rev 7802) +++ code/player/trunk/server/drivers/mixed/p2os/p2os.cc 2009-06-06 10:23:35 UTC (rev 7803) @@ -355,6 +355,7 @@ memset(&this->audio_id, 0, sizeof(player_devaddr_t)); memset(&this->actarray_id, 0, sizeof(player_devaddr_t)); memset(&this->limb_id, 0, sizeof(player_devaddr_t)); + memset(&this->ptz_id, 0, sizeof(player_devaddr_t)); this->position_subscriptions = this->sonar_subscriptions = this->actarray_subscriptions = 0; this->pulse = -1; @@ -496,6 +497,19 @@ } } + // Do We create the PTZ Interface + if(cf->ReadDeviceAddr(&(this->ptz_id), section, "provides", + PLAYER_PTZ_CODE, -1, NULL) == 0) + { + if(this->AddInterface(this->ptz_id) != 0) + { + this->SetError(-1); + return; + } + this->minfov = (int)rint(RTOD(cf->ReadTupleAngle(section, "fov", 0, DTOR(3)))); + this->maxfov = (int)rint(RTOD(cf->ReadTupleAngle(section, "fov", 1, DTOR(30)))); + } + // Do we create a limb interface? if(cf->ReadDeviceAddr(&(this->limb_id), section, "provides", PLAYER_LIMB_CODE, -1, NULL) == 0) { @@ -1278,7 +1292,12 @@ } } + // Set up the PTZ Camera + if(this->ptz_id.interf){ + SetupPtz(); + } + // TODO: figure out what the right behavior here is #if 0 // zero position command buffer @@ -1291,6 +1310,11 @@ return(0); } + + + + + void P2OS::MainQuit() { unsigned char command[20],buffer[20]; @@ -1301,6 +1325,16 @@ if(this->psos_fd == -1) return; + // Shut Down the PTZ camera + if(this->ptz_id.interf){ + usleep(PTZ_SLEEP_TIME_USEC); + SendAbsPanTilt(0,0); + usleep(PTZ_SLEEP_TIME_USEC); + SendAbsZoom(0); + setPower(0); + puts("PTZ camera has been shutdown"); + } + command[0] = STOP; packet.Build(command, 1); packet.Send(this->psos_fd); @@ -1473,6 +1507,12 @@ (void*)&(this->p2os_data.compass), sizeof(player_position2d_data_t), ×tampStandardSIP); + + // put PTZ data + this->Publish(this->ptz_id, + PLAYER_MSGTYPE_DATA, + PLAYER_PTZ_DATA_STATE, + (void*)&(this->ptz_data)); } void @@ -1599,6 +1639,44 @@ } } + if(this->ptz_id.interf) + { + int pan,tilt; + int zoom; + //fprintf(stderr, "PTZ MAIN LOOP\n"); + //fprintf(stderr, "Getting Pan/Tilt Data\n"); + if(GetAbsPanTilt(&pan,&tilt) < 0) + { + fputs("canonvcc4:Main():GetAbsPanTilt() errored. bailing.\n", + stderr); + pthread_exit(NULL); + } + + usleep(30000); + //fprintf(stderr, "Getting Zoom Data\n"); + zoom = 0; + if(GetAbsZoom(&zoom) < 0) + { + fputs("canonvcc4:Main():GetAbsZoom() errored. bailing.\n", stderr); + pthread_exit(NULL); + } + // Do the necessary coordinate conversions. Camera's natural pan + // coordinates increase clockwise; we want them the other way, so + // we negate pan here. Zoom values are converted from arbitrary + // units to a field of view (in degrees). + pan = -pan; + + //printf("before zoom = %i\n", zoom); + //zoom = this->maxfov + (zoom * (this->maxfov - this->minfov)) / maxzoom; + //printf("after zoom = %i\n", zoom); + + ptz_data.pan = DTOR((unsigned short)pan); + ptz_data.tilt = DTOR((unsigned short)tilt); + + //double m = (double)(this->minfov - this->maxfov) / (double)this->maxzoom; + ptz_data.zoom = DTOR(this->maxfov+(zoom * (double)(this->minfov - this->maxfov) / (double)this->maxzoom)); + } + // handle pending messages if(!this->InQueue->Empty()) { @@ -1664,6 +1742,29 @@ packet.packet[3] == SERAUX) { // This is an AUX serial packet + if(ptz_id.interf) + { + /* It is an extended SIP (ptz) packet, so process it */ + /* Be sure to pass data size too (packet[2])! */ + //fprintf(stderr,"Got a PTZ packet\n"); + int len; + //printf("Got ptz packet\n"); + + len = packet.packet[2] - 3; + //packet.PrintHex(); + //fprintf(stderr, "Got PTZ Packet of length %i\n",len); + + if ( cb.gotPacket() ){ + fprintf(stderr, "ptz_error: got a message, but we already have the complete packet.\n"); + } + else { + for ( int i = 4; i < 4+len ; i++ ) + { + cb.putOnBuf(packet.packet[i]); + } + } + + } } else if(packet.packet[0] == 0xFA && packet.packet[1] == 0xFB && packet.packet[3] == SERAUX2) @@ -2598,11 +2699,41 @@ this->Publish(this->armgripper_id, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_GRIPPER_REQ_GET_GEOM, &geom, sizeof (geom), NULL); return 0; } + // PTZ Stuff now. + else if (Message::MatchMessage(hdr, + PLAYER_MSGTYPE_REQ, + PLAYER_PTZ_REQ_GENERIC, device_addr)) + { + assert(hdr->size == sizeof(player_ptz_req_generic_t)); + + player_ptz_req_generic_t *cfg = (player_ptz_req_generic_t *)data; + + // check whether command or inquiry... + if (cfg->config[0] == 0x01) + { + if (SendCommand((uint8_t *)cfg->config, cfg->config_count) < 0) + Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, hdr->subtype); + else + Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype); + return 0; + } + else + { + // this is an inquiry, so we have to send data back + cfg->config_count = SendRequest((uint8_t*)cfg->config, + cfg->config_count, + (uint8_t*)cfg->config); + Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype); + } + return 0; + } else { PLAYER_WARN("unknown config request to p2os driver"); return(-1); } + + return 0; } void P2OS::SendPulse (void) @@ -3049,6 +3180,7 @@ return -1; } + /////////////////////////////////////////////////////////////////////////////// // Lift commands @@ -3101,6 +3233,7 @@ // becomes travel time / 0.02. // It is important to remember that the LIFTcarry is (if my reading of the manul is correct) // an offset command, not absolute position command. So we have to track the last commanded + // position of the lift and work from that to get the correct travel time (and direction). double offset = cmd.position - lastLiftPosCmd.position; @@ -3371,3 +3504,957 @@ } return retVal; } + + +// I'm going to put my PTZ stuff down here for now. +/************************************************************************/ + +int +P2OS::SetupPtz() +{ + int err; + int error_code; + int pan, tilt; + fprintf(stderr, "Setting up the Canon PTZ Camera.\n"); + + //err = setPower(0); + + do { + //fprintf(stderr, "\nPowering off/on the camera!!!!!!!!!!!!!!\n"); + fprintf(stderr, "\nPowering On the Camera.\n"); + err = setPower(1); + while (error_code == CAM_ERROR_BUSY) + { + fprintf(stdout, "power on busy: %x\n", error_code); + err = setPower(1); + } + if ((err != 0) && + (error_code != CAM_ERROR_NONE) && (error_code != CAM_ERROR_MODE)) + { + printf("Could not set power on: %x\n", error_code); + setPower(0); + //close(ptz_fd); + return -1; + } + + /* Set Host Mode Control */ + fprintf(stderr, "\nSeting Host Control mode.\n"); + err = setControlMode(); + while (error_code == CAM_ERROR_BUSY) + { + printf("control mode busy: %x\n", error_code); + err = setControlMode(); + } + if (err) + { + printf("Could not set control mode\n"); + setPower(0); + //close(ptz_fd); + return -1; + } + + + /* Send Init Command */ + fprintf(stderr, "\nSendInit()\n"); + err = sendInit(); + while (error_code == CAM_ERROR_BUSY) + { + fprintf(stdout, "sendInit busy: %x\n", error_code); + err = sendInit(); + } + if ((err != 0) && (error_code != CAM_ERROR_NONE) && (error_code != CAM_ERROR_MODE)) + { + printf("Could not sendInit off: %x\n", error_code); + setPower(0); + return -1; + } + }while (error_code == CAM_ERROR_MODE); + + /* Turn on Notify Command */ + /* + fprintf(stderr, "Setting Notify Command on\n"); + err = setNotifyCommand(); + while (error_code == CAM_ERROR_BUSY) + { + printf("notify command busy: %x\n", error_code); + err = setNotifyCommand(); + } + if (err) + { + printf("Could not set notify command\n"); + setPower(0); + //close(ptz_fd); + return -1; + } + */ + + /* Now Turn Power on. */ + /* + fprintf(stderr, "Powering On the Camera.\n"); + err = setPower(1); + while (error_code == CAM_ERROR_BUSY) + { + fprintf(stdout, "power on busy: %x\n", error_code); + err = setPower(1); + } + if ((err != 0) && + (error_code != CAM_ERROR_NONE) && (error_code != CAM_ERROR_MODE)) + { + printf("Could not set power on: %x\n", error_code); + setPower(0); + return -1; + } + */ + fprintf(stderr, "\nSetting the default tilt range.\n"); + err = setDefaultTiltRange(); + while (error_code == CAM_ERROR_BUSY) + { + printf("control mode busy: %x\n", error_code); + err = setDefaultTiltRange(); + } + if (err) + { + printf("Could not set default tilt range\n"); + setPower(0); + //close(ptz_fd); + return -1; + } + + /* try to get current state, just to make sure we actually have a camera */ + fprintf(stderr, "\nGetting the Abs Pan Tilt\n"); + err = GetAbsPanTilt(&pan,&tilt); + if (err) + { + printf("Couldn't connect to PTZ device most likely because the camera\n" + "is not connected or is connected not to AUX1: %x\n", + error_code); + setPower(0); + //close(ptz_fd); + //ptz_fd = -1; + return(-1); + } + fprintf(stderr, "getAbsPantilt: %d %d\n", pan, tilt); + + // Get the Zoom Range. 0 to what + fprintf(stderr, "Getting Max Zoom Range.\n"); + err = GetMaxZoom(&maxzoom); + if (err) + { + fprintf(stderr, "Couldn't get max zoom range.\n"); + setPower(0); + //close(ptz_fd); + //ptz_fd = -1; + return(-1); + } + fprintf(stderr, "maxzoom value = %i \n", maxzoom); + fprintf(stderr, "Done Initializing the PTZ Camera.\n"); + return 0; +} + + +int +P2OS::SendCommand(unsigned char *str, int len) +{ + int err = 0; + P2OSPacket ptz_packet; + P2OSPacket request_pkt; + unsigned char request[4]; + + // Zero out the Receive Buffer + request[0] = GETAUX; + request[1] = ARGINT; + request[2] = 0; + request[3] = 0; + request_pkt.Build(request,4); + SendReceive(&request_pkt,false); + + if(len > MAX_PTZ_COMMAND_LENGTH) + { + fprintf(stderr, + "CANNONvcc4::SendCommand(): message is too large (%d bytes)\n", + len); + return(-1); + } + + //err = write(ptz_fd, str, len); + // Since I'm hardcoding this to AUX1, basically we gotta stick the AUX1DATA + // header on this and then give it to the p2os send command. + unsigned char mybuf[MAX_PTZ_COMMAND_LENGTH]; + mybuf[0] = TTY2; + mybuf[1] = ARGSTR; + mybuf[2] = len; + // Copy the command + memcpy(&mybuf[3], str, len); + //for (int i = 0; i < len+3; i ++) + // fprintf(stderr, "0x%x ", mybuf[i]); + //fprintf(stderr,"\n"); + ptz_packet.Build(mybuf, len+3); + + /* + printf("ptz_command packet = "); + ptz_packet.PrintHex(); + */ + + // Send the packet + this->SendReceive(&ptz_packet, false); + + if (err != 0) + { + perror("canonvcc4::Send():write():"); + return(-1); + } + return(0); +} +/************************************************************************/ +//int +//canonvcc4::SendRequest(unsigned char* str, int len) + +int P2OS::SendRequest(unsigned char* str, int len, unsigned char* reply, uint8_t camera) +{ + return this->SendCommand(str,len); +} + +/************************************************************************/ +void +P2OS::PrintPacket(char* str, unsigned char* cmd, int len) +{ + for(int i=0;i<len;i++) + printf(" %.2x", cmd[i]); + puts(""); +} +/************************************************************************/ + +int +P2OS::SendAbsPanTilt(int pan, int tilt) +{ + unsigned char command[MAX_PTZ_COMMAND_LENGTH]; + int convpan, convtilt; + unsigned char buf[5]; + int ppan, ttilt; + + ppan = pan; ttilt = tilt; + if (abs(pan) > PTZ_PAN_MAX) + { + if(pan < -PTZ_PAN_MAX) + ppan = (int)-PTZ_PAN_MAX; + else + if(pan > PTZ_PAN_MAX) + ppan = (int)PTZ_PAN_MAX; + //puts("Camera pan angle thresholded"); + } + if (tilt > PTZ_TILT_MAX) + ttilt = (int)PTZ_TILT_MAX; + else + if(tilt < PTZ_TILT_MIN) + ttilt = (int)PTZ_TILT_MIN; + //puts("Camera pan angle thresholded"); + + //puts("Camera tilt angle thresholded"); + + convpan = (int)floor(ppan/.1125) + 0x8000; + convtilt = (int)floor(ttilt/.1125) + 0x8000; +// fprintf(stdout, "ppan: %d ttilt: %d conpan: %d contilt: %d\n", +// ppan,ttilt,convpan,convtilt); + command[0] = 0xFF; + command[1] = 0x30; + command[2] = 0x30; + command[3] = 0x00; + command[4] = 0x62; + // pan position + + sprintf((char *)buf, "%X", convpan); + + command[5] = buf[0]; + command[6] = buf[1]; + command[7] = buf[2]; + command[8] = buf[3]; + // tilt position + sprintf((char *)buf, "%X", convtilt); + command[9] = buf[0]; + command[10] = buf[1]; + command[11] = buf[2]; + command[12] = buf[3]; + command[13] = (unsigned char) 0xEF; + SendCommand(command, 14); + // PrintPacket( "sendabspantilt: ", command, 14); + return(ReceiveCommandAnswer(6)); +} +/************************************************************************/ +int +P2OS::setDefaultTiltRange() +{ + unsigned char command[MAX_PTZ_COMMAND_LENGTH]; + unsigned char buf[8]; + int maxtilt, mintilt; + + command[0] = 0xFF; + command[1] = 0x30; + command[2] = 0x30; + command[3] = 0x00; + command[4] = 0x64; + command[5] = 0x31; + + mintilt = (int)(floor(PTZ_TILT_MIN/.1125) + 0x8000); + sprintf((char *)buf, "%X", mintilt); + + command[6] = buf[0]; + command[7] = buf[1]; + command[8] = buf[2]; + command[9] = buf[3]; + // tilt position + maxtilt = (int)(floor(PTZ_TILT_MAX/.1125) + 0x8000); + sprintf((char *)buf, "%X", maxtilt); + + command[10] = buf[0]; + command[11] = buf[1]; + command[12] = buf[2]; + command[13] = buf[3]; + command[14] = (unsigned char) 0xEF; + + SendCommand(command, 15); + // PrintPacket( "setDefaultRange: ", command, 15); + return(ReceiveCommandAnswer(6)); + +} + +/************************************************************************/ +int +P2OS::GetAbsPanTilt(int* pan, int* tilt) +{ + unsigned char command[MAX_PTZ_COMMAND_LENGTH]; + unsigned char reply[MAX_PTZ_REQUEST_LENGTH]; + int reply_len; + unsigned char buf[4]; + char byte; + unsigned int u_val; + int val; + int i; + + command[0] = 0xFF; + command[1] = 0x30; + command[2] = 0x30; + command[3] = 0x00; + command[4] = 0x63; + command[5] = 0xEF; + + if (SendRequest(command, 6, reply)) + return(-1); + // PrintPacket("getabspantilt: ", command, 6); + //reply_len = ReceiveRequestAnswer(reply,6,14); + reply_len = ReceiveRequestAnswer(reply,14,0); + + if ( reply_len != 14 ) { + fprintf(stderr, "Reply Len = %i\n", reply_len); + return -1; + } + + // remove the ascii encoding, and put into 4-byte array + for (i = 0; i < 4; i++) + { + byte = reply[i+5]; + if (byte < 0x40) + byte = byte - 0x30; + else + byte = byte - 'A' + 10; + buf[i] = byte; + } + + // convert the 4-bytes into a number + u_val = buf[0] * 0x1000 + buf[1] * 0x100 + buf[2] * 0x10 + buf[3]; + + // convert the number to a value that's meaningful, based on camera specs + val = (int)(((int)u_val - (int)0x8000) * 0.1125); + + // now set myPan to the response received for where the camera thinks it is + *pan = val; + + // repeat the steps for the tilt value + for (i = 0; i < 4; i++) + { + byte = reply[i+9]; + if (byte < 0x40) + byte = byte - 0x30; + else + byte = byte - 'A' + 10; + buf[i] = byte; + } + u_val = buf[0] * 0x1000 + buf[1] * 0x100 + buf[2] * 0x10 + buf[3]; + val =(int)(((int)u_val - (int)0x8000) * 0.1125); + *tilt = val; + + return(0); +} + +/************************************************************************/ +int +P2OS::GetAbsZoom(int* zoom) +{ + unsigned char command[MAX_PTZ_COMMAND_LENGTH]; + unsigned char reply[MAX_PTZ_REQUEST_LENGTH]; + int reply_len; + char byte; + unsigned char buf[4]; + unsigned int u_zoom; + int i; + + command[0] = 0xFF; + command[1] = 0x30; + command[2] = 0x30; + command[3] = 0x00; + command[4] = 0xB4; + command[5] = 0x30; + command[6] = 0xEF; + + if (SendRequest(command, 7, reply)) + return(-1); + // PrintPacket( "getabszoom: ", command, 6); + + reply_len = ReceiveRequestAnswer(reply,10,0); + + if (reply_len == 6) + return -1; + + // remove the ascii encoding, and put into 2 bytes + for (i = 0; i < 4; i++) + { + byte = reply[i + 5]; + if (byte < 0x40) + byte = byte - 0x30; + else + byte = byte - 'A' + 10; + buf[i] = byte; + } + + // convert the 2 bytes into a number + u_zoom = 0; + for (i = 0; i < 4; i++) + u_zoom += buf[i] * (unsigned int) pow(16.0, (double)(3 - i)); + *zoom = u_zoom; + return(0); +} + +/************************************************************************/ +int +P2OS::SendAbsZoom(int zoom) +{ + unsigned char command[MAX_PTZ_COMMAND_LENGTH]; + unsigned char buf[5]; + int i; + + if(zoom < 0) + zoom = 0; + //puts("Camera zoom thresholded"); + + else + if(zoom > maxzoom){ + zoom = maxzoom; + //printf("putting zoom at MAX_ZOOM\n"); + } + + command[0] = 0xFF; + command[1] = 0x30; + command[2] = 0x30; + command[3] = 0x00; + command[4] = 0xB3; + + sprintf((char *)buf, "%4X", zoom); + + for (i=0;i<3;i++) + if (buf[i] == ' ') + buf[i] = '0'; + + // zoom position + command[5] = buf[0]; + command[6] = buf[1]; + command[7] = buf[2]; + command[8] = buf[3]; + command[9] = 0xEF; + + if (SendCommand(command, 10)) + return -1; + //PrintPacket( "setabszoom: ", command, 10); + return (ReceiveCommandAnswer(6)); +} + +void P2OS::get_ptz_packet(int s1, int s2) +{ + //printf("get_ptz_packet()\n"); + static const int TIMEOUT = 100; + int packetCount = 0; + unsigned char request[4]; + P2OSPacket request_pkt; + bool secondSent = false; + + request[0] = GETAUX; + request[1] = ARGINT; + request[2] = s1; + request[3] = 0; + + // Reset our receiving buffer. + cb.reset(); + + //sleep(1); + //Request the request-size back + request_pkt.Build(request,4); + SendReceive(&request_pkt,false); + + while ( !cb.gotPacket() ) + { + if ( packetCount++ > TIMEOUT ) { + // Give Up We're not getting it. + fprintf(stderr, "Waiting for packet timed out.\n"); + return; + } + if ( cb.size() == s1 && !secondSent) + { + if ( s2 > s1 ) + { + // We got the first packet size, but we don't have a full packet. + int newsize = s2 - s1; + fprintf(stderr, "Requesting Second Packet of size %i\n", newsize); + request[2] = newsize; + request_pkt.Build(request,4); + secondSent = true; + //cb.printBuf(); + SendReceive(&request_pkt,false); + } + else + { + // We got the first packet but don't have a full packet, this is an error. + fprintf(stderr, "Error: Got reply from AUX1 But don't have a full packet.\n"); + break; + } + } + + // Keep reading data until we get a response from the camera. + //fprintf(stderr, "foo\n"); + SendReceive(NULL,false); + } + //fprintf(stderr, "Got ptz_packet: "); + //cb.printBuf(); +} + +/************************************************************************/ +int +P2OS::ReceiveCommandAnswer(int asize) +{ + int num; + unsigned char reply[MAX_PTZ_REQUEST_LENGTH]; + int len = 0; + unsigned char byte; + int t; + int error_code; + // puts("Receivecommandanswer begin\n"); + memset(reply, 0, COMMAND_RESPONSE_BYTES); + + get_ptz_packet(asize); + //fprintf(stderr, "Recieved Packet: "); + //cb.printBuf(); + + for (num = 0; num <= COMMAND_RESPONSE_BYTES + 1; num++) + { + // if we don't get any bytes, or if we've just exceeded the limit + // then return null + //err = read_ptz(&byte, 1); + t = cb.getFromBuf(); + if ( t < 0 ) { // Buf Error! + printf("circbuf error!\n"); + } + else { + byte = (unsigned char)t; + } + if (byte == (unsigned char)0xFE) + { + reply[0] = byte; + len ++; + break; + } + } + if (len == 0) + return -1; + + // we got the header character so keep reading bytes for MAX_RESPONSE_BYTES more + for(num = 1; num <= MAX_PTZ_REQUEST_LENGTH; num++) + { + t = cb.getFromBuf(); + if (t < 0) + { + // there are no more bytes, so check the last byte for the footer + if (reply[len - 1] != (unsigned char)0xEF) + { + fputs("canonvcc4::receiveCommandAnswer: Discarding bad packet.", + stderr); + return -1; + } + else + break; + } + else + { + // add the byte to the array + reply[len] = (unsigned char)t; + len ++; + } + } + + // Check the response + if (len != 6) + { + fputs("canonvcc4::receiveCommandAnswer:Incorrect number of bytes in response packet.", + stderr); + return -1; + } + + // check the header and footer + if (reply[0] != (unsigned char)0xFE || reply[5] != (unsigned char)0xEF) + { + fputs("canonvcc4::receiveCommandAnswer: Bad header or footer character in response packet.", stderr); + return -1; + } + // so far so good. Set myError to the error byte + error_code = reply[3]; + //PrintPacket("receivecommandasnwer: ", reply, 6); + if (error_code == CAM_ERROR_NONE) + return 0; + else { + switch(error_code){ + case CAM_ERROR_BUSY: + fputs("Error: CAM_ERROR_BUSY\n", stderr); + break; + case CAM_ERROR_PARAM: + fputs("Error: CAM_ERROR_PARAM\n", stderr); + break; + case CAM_ERROR_MODE: + fputs("Error: CAM_ERROR_MODE\n", stderr); + break; + default: + fputs("Error: Unknown error response from camera.\n",stderr); + break; + } + } + + return -1; +} + +/************************************************************************/ +/* These commands often have variable packet lengths, if there is an error, + * there is a smaller packet size. If we request the larger packet size first, + * then we will never get a response back. Because of this, we have to first + * request the smaller size, check if its a full packet, if it's not, request + * the rest of the packet. Also according to the source code for ARIA, we can + * not do more than 2 requests for a single packet, therefor, we can't just + * request 1 byte over and over again. + * + * So here, s1 is the size of the smaller packet. + * And s2 is the size of the larger packet. + */ +int +P2OS::ReceiveRequestAnswer(unsigned char *data, int s1, int s2) +{ + int num; + unsigned char reply[MAX_PTZ_REQUEST_LENGTH]; + int len = 0; + unsigned char byte; + int t; + int error_code; + + memset(reply, 0, MAX_PTZ_REQUEST_LENGTH); + get_ptz_packet(s1, s2); + //cb.printBuf(); + + for (num = 0; num <= COMMAND_RESPONSE_BYTES + 1; num++) + { + // if we don't get any bytes, or if we've just exceeded the limit + // then return null + t = cb.getFromBuf(); + if ( t < 0 ) { // Buf Error! + printf("circbuf error!\n"); + } + else { + byte = (unsigned char)t; + } + if (byte == (unsigned char)0xFE) + { + reply[0] = byte; + len ++; + break; + } + } + if (len == 0) + return -1; + // we got the header character so keep reading bytes for MAX_RESPONSE_BYTES more + for(num = 1; num <= MAX_PTZ_REQUEST_LENGTH; num++) + { + t = cb.getFromBuf(); + if (t < 0) + { + // there are no more bytes, so check the last byte for the footer + if (reply[len - 1] != (unsigned char)0xEF) + { + fputs("canonvcc4::receiveRequest: Discarding bad packet.", + stderr); + return -1; + } + else + break; + } + else + { + // add the byte to the array + reply[len] = (unsigned char)t; + len ++; + } + } + // Check the response length: pt: 14; zoom: 10 + if (len != 6 && len != 8 && len != 10 && len != 14) + { + fputs("Arvcc4::packetHandler: Incorrect number of bytes in response packet.", stderr); + return -1; + } + + if (reply[0] != (unsigned char)0xFE || + reply[len - 1] != (unsigned char)0xEF) + { + fputs("canonvcc4::receiveRequestArvcc4: Bad header or footer character in response packet.", stderr); + return -1; + } + + // so far so good. Set myError to the error byte + error_code = reply[3]; + // PrintPacket("receiverequestasnwer: ", reply, len); + if (error_code == CAM_ERROR_NONE) + { + memcpy(data, reply, len); + return len; + } + return -1; +} + +/************************************************************************/ +int +P2OS::setControlMode() +{ + unsigned char command[MAX_PTZ_COMMAND_LENGTH]; + + command[0] = 0xFF; + command[1] = 0x30; + command[2] = 0x30; + command[3] = 0x00; + command[4] = 0x90; + command[5] = 0x30; + command[6] = 0xEF; + + if (SendCommand(command, 7)) + return -1; + // usleep(5000000); + return (ReceiveCommandAnswer(6)); +} +/************************************************************************/ +int +P2OS::setNotifyCommand() +{ + unsigned char command[MAX_PTZ_COMMAND_LENGTH]; + + command[0] = 0xFF; + command[1] = 0x30; + command[2] = 0x30; + command[3] = 0x00; + command[4] = 0x94; + command[5] = 0x31; + command[6] = 0xEF; + + if (SendCommand(command, 7)) + return -1; + // usleep(5000000); + return (ReceiveCommandAnswer(6)); +} +/************************************************************************/ +int +P2OS::setPower(int on) +{ + unsigned char command[MAX_PTZ_COMMAND_LENGTH]; + + command[0] = 0xFF; + command[1] = 0x30; + command[2] = 0x30; + command[3] = 0x00; + command[4] = 0xA0; + if (on) + command[5] = 0x31; + else + command[5] = 0x30; + command[6] = 0xEF; + + if (SendCommand(command, 7)) + return -1; + // usleep(5000000); + return (ReceiveCommandAnswer(6)); +} +/************************************************************************/ +int +P2OS::setOnScreenOff() +{ + unsigned char command[MAX_PTZ_COMMAND_LENGTH]; + + command[0] = 0xFF; + command[1] = 0x30; + command[2] = 0x30; + command[3] = 0x00; + command[4] = 0x91; + command[5] = 0x30; + command[6] = 0x30; + command[7] = 0xEF; + + if (SendCommand(command, 8)) + return -1; + // usleep(5000000); + return (ReceiveCommandAnswer(6)); +} + +int P2OS::sendInit() +{ + unsigned char command[MAX_PTZ_COMMAND_LENGTH]; + + command[0] = 0xFF; + command[1] = 0x30; + command[2] = 0x30; + command[3] = 0x00; + command[4] = 0x58; + command[5] = 0x30; + command[6] = 0xEF; + + if (SendCommand(command, 7)) + return -1; + // usleep(5000000); + return (ReceiveCommandAnswer(6)); +} + +int P2OS::GetMaxZoom(int * maxzoom){ + unsigned char command[MAX_PTZ_COMMAND_LENGTH]; + unsigned char reply[MAX_PTZ_REQUEST_LENGTH]; + int reply_len; + char byte; + unsigned char buf[4]; + unsigned int u_zoom; + int i; + + command[0] = 0xFF; + command[1] = 0x30; + command[2] = 0x30; + command[3] = 0x00; + command[4] = 0xB4; + command[5] = 0x33; + command[6] = 0xEF; + + if (SendCommand(command, 7)) + return -1; + // usleep(5000000); + + reply_len = ReceiveRequestAnswer(reply,10,0); + + if ( reply_len == 6 ){ + return -1; + } + + // remove the ascii encoding, and put into 2 bytes + for (i = 0; i < 4; i++) + { + byte = reply[i + 5]; + if (byte < 0x40) + byte = byte - 0x30; + else + byte = byte - 'A' + 10; + buf[i] = byte; + } + + // convert the 2 bytes into a number + u_zoom = 0; + for (i = 0; i < 4; i++) + u_zoom += buf[i] * (unsigned int) pow(16.0, (double)(3 - i)); + *maxzoom = u_zoom; + + return 0; +} + +/* Circular Buffer To deal with getting data back from AUX */ +circbuf::circbuf(int size) +{ + this->buf = new unsigned char[size]; + this->mysize = size; + this->start = this->end = 0; +} + +void circbuf::printBuf(){ + int i = start; + printf("circbuf: "); + while ( i != end ){ + printf("0x%x ", buf[i]); + i = (i+1)%mysize; + } + printf("\n"); +} + + +void circbuf::putOnBuf(unsigned char c) +{ + buf[end] = c; + end = (end+1)%mysize; + if ( end == start ) + { + // We're overwriting old data. + start = (start + 1)%mysize; + } + + // Check to see if we have the whole packet now. (ends with 0xEF) + if ( c == 0xEF ) + { + gotPack = true; + } +} + +bool circbuf::haveData() +{ + return !(this->start == this->end); +} + +int circbuf::getFromBuf() +{ + if ( start != end ){ + unsigned char ret = buf[start]; + start = (start+1)%mysize; + return (int)ret; + } + else + { + return -1; + } +} + +int circbuf::size() +{ + if ( end > start ) + { + return end-start; + } + else if ( start > end ) + { + return mysize - start - end - 1; + } + else + { + return 0; + } +} + +bool circbuf::gotPacket() +{ + return gotPack; +} + +void circbuf::reset() +{ + memset(buf, 0, mysize); + gotPack = false; + start = end = 0; +} + Modified: code/player/trunk/server/drivers/mixed/p2os/p2os.h =================================================================== --- code/player/trunk/server/drivers/mixed/p2os/p2os.h 2009-06-06 09:51:04 UTC (rev 7802) +++ code/player/trunk/server/drivers/mixed/p2os/p2os.h 2009-06-06 10:23:35 UTC (rev 7803) @@ -41,6 +41,7 @@ #include "packet.h" #include "robot_params.h" + // Default max speeds #define MOTOR_DEF_MAX_SPEED 0.5 #define MOTOR_DEF_MAX_TURNSPEED DTOR(100) @@ -143,6 +144,28 @@ #define DEFAULT_P2OS_TCP_REMOTE_HOST "localhost" #define DEFAULT_P2OS_TCP_REMOTE_PORT 8101 +/* Canon PTZ (VC-C4) Stuff */ +#define CAM_ERROR_NONE 0x30 +#define CAM_ERROR_BUSY 0x31 +#define CAM_ERROR_PARAM 0x35 +#define CAM_ERROR_MODE 0x39 + +#define PTZ_SLEEP_TIME_USEC 100000 + +#define MAX_PTZ_COMMAND_LENGTH 19 +#define MAX_PTZ_REQUEST_LENGTH 17 +#define COMMAND_RESPONSE_BYTES 6 + +#define PTZ_PAN_MAX 98.0 // 875 units 0x36B +#define PTZ_TILT_MAX 88.0 // 790 units 0x316 +#define PTZ_TILT_MIN -30.0 // -267 units 0x10B +#define MAX_ZOOM 1960 //1900 +#define ZOOM_CONV_FACTOR 17 + +#define PT_BUFFER_INC 512 +#define PT_READ_TIMEOUT 10000 +#define PT_READ_TRIALS 2 + typedef struct player_p2os_data { //Standard SIP @@ -174,6 +197,28 @@ class SIP; +// Circular Buffer Used by PTZ camera +class circbuf{ + public: + circbuf(int size=512); + + void putOnBuf(unsigned char c); + int getFromBuf(); + bool haveData(); + int size(); + void printBuf(); + + bool gotPacket(); + void reset(); + + private: + unsigned char* buf; + int start; + int end; + int mysize; + bool gotPack; +}; + // Forward declaration of the KineCalc_Base class declared in kinecalc_base.h //class KineCalc; @@ -196,6 +241,7 @@ player_devaddr_t audio_id; player_devaddr_t actarray_id; player_devaddr_t limb_id; + player_devaddr_t ptz_id; player_devaddr_t armgripper_id; // Book keeping to only send new commands @@ -219,6 +265,7 @@ int position_subscriptions; int sonar_subscriptions; int actarray_subscriptions; + int ptz_subscriptions; SIP* sippacket; @@ -242,6 +289,10 @@ int HandleArmGripperCommand (player_msghdr *hdr, void *data); void HandleAudioCommand(player_audio_sample_item_t audio_cmd); + /////// PTZ Stuff + void get_ptz_packet(int s1, int s2=0); + int SetupPtz(); + ///////////////// // Gripper stuff player_pose3d_t gripperPose; @@ -314,6 +365,31 @@ int joystickp; // are we using a joystick? int bumpstall; // should we change the bumper-stall behavior? + // PTZ Camera Stuff + player_ptz_data_t ptz_data; + int maxfov, minfov; + int maxzoom; + int pandemand, tiltdemand, zoomdemand; + int SendCommand(unsigned char *str, int len); + int SendRequest(unsigned char* str, int len, unsigned char* reply, uint8_t camera = 1); + void PrintPacket(char* str, unsigned char* cmd, int len); + int SendAbsPanTilt(int pan, int tilt); + int setDefaultTiltRange(); + int GetAbsPanTilt(int* pan, int* tilt); + int GetAbsZoom(int* zoom); + int SendAbsZoom(int zoom); + int read_ptz(unsigned char *reply, int size); + int ReceiveCommandAnswer(int asize); + int ReceiveRequestAnswer(unsigned char *data, int s1, int s2); + int setControlMode(); + int setNotifyCommand(); + int setPower(int on); + int setOnScreenOff(); + int CheckHostControlMode(); + int sendInit(); + int GetMaxZoom(int * maxzoom); + circbuf cb; + float pulse; // Pulse time double lastPulseTime; // Last time of sending a pulse or command to the robot void SendPulse (void); Modified: code/player/trunk/server/drivers/shell/writelog.cc =================================================================== --- code/player/trunk/server/drivers/shell/writelog.cc 2009-06-06 09:51:04 UTC (rev 7802) +++ code/player/trunk/server/drivers/shell/writelog.cc 2009-06-06 10:23:35 UTC (rev 7803) @@ -2275,7 +2275,7 @@ fiducial_data = (player_fiducial_data_t*) data; // format: <count> [<id> <x> <y> <z> <roll> <pitch> <yaw> <ux> <uy> <uz> <uroll> <upitch> <uyaw>] ... fprintf(this->file, "%d", fiducial_data->fiducials_count); - for (int i = 0; i < fiducial_data->fiducials_count; i++) { + for (unsigned i = 0; i < fiducial_data->fiducials_count; i++) { fprintf(this->file, " %d" " %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f" " %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f", This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2009-06-07 08:11:01
|
Revision: 7808 http://playerstage.svn.sourceforge.net/playerstage/?rev=7808&view=rev Author: thjc Date: 2009-06-07 08:10:31 +0000 (Sun, 07 Jun 2009) Log Message: ----------- applied patch 2107219: added serio and camfilter drivers Modified Paths: -------------- code/player/trunk/server/drivers/CMakeLists.txt code/player/trunk/server/drivers/camera/CMakeLists.txt Added Paths: ----------- code/player/trunk/server/drivers/camera/camfilter/ code/player/trunk/server/drivers/camera/camfilter/AUTHORS code/player/trunk/server/drivers/camera/camfilter/CMakeLists.txt code/player/trunk/server/drivers/camera/camfilter/COPYING code/player/trunk/server/drivers/camera/camfilter/camfilter.cc code/player/trunk/server/drivers/camera/camfilter/version.txt code/player/trunk/server/drivers/dio/ code/player/trunk/server/drivers/dio/CMakeLists.txt code/player/trunk/server/drivers/dio/serio/ code/player/trunk/server/drivers/dio/serio/AUTHORS code/player/trunk/server/drivers/dio/serio/CMakeLists.txt code/player/trunk/server/drivers/dio/serio/COPYING code/player/trunk/server/drivers/dio/serio/serio.cc code/player/trunk/server/drivers/dio/serio/version.txt Modified: code/player/trunk/server/drivers/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/CMakeLists.txt 2009-06-07 07:44:40 UTC (rev 7807) +++ code/player/trunk/server/drivers/CMakeLists.txt 2009-06-07 08:10:31 UTC (rev 7808) @@ -7,6 +7,7 @@ ADD_SUBDIRECTORY (blackboard) ADD_SUBDIRECTORY (blobfinder) ADD_SUBDIRECTORY (camera) +ADD_SUBDIRECTORY (dio) ADD_SUBDIRECTORY (fiducial) ADD_SUBDIRECTORY (gps) ADD_SUBDIRECTORY (health) Modified: code/player/trunk/server/drivers/camera/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/camera/CMakeLists.txt 2009-06-07 07:44:40 UTC (rev 7807) +++ code/player/trunk/server/drivers/camera/CMakeLists.txt 2009-06-07 08:10:31 UTC (rev 7808) @@ -1,5 +1,6 @@ ADD_SUBDIRECTORY (v4l) ADD_SUBDIRECTORY (1394) +ADD_SUBDIRECTORY (camfilter) ADD_SUBDIRECTORY (compress) ADD_SUBDIRECTORY (cvcam) ADD_SUBDIRECTORY (imageseq) Added: code/player/trunk/server/drivers/camera/camfilter/AUTHORS =================================================================== --- code/player/trunk/server/drivers/camera/camfilter/AUTHORS (rev 0) +++ code/player/trunk/server/drivers/camera/camfilter/AUTHORS 2009-06-07 08:10:31 UTC (rev 7808) @@ -0,0 +1 @@ +Paul Osmialowski <new...@ki...> Added: code/player/trunk/server/drivers/camera/camfilter/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/camera/camfilter/CMakeLists.txt (rev 0) +++ code/player/trunk/server/drivers/camera/camfilter/CMakeLists.txt 2009-06-07 08:10:31 UTC (rev 7808) @@ -0,0 +1,7 @@ +PLAYERDRIVER_OPTION (camfilter build_camfilter ON) +IF (HAVE_JPEG) + PLAYERDRIVER_REQUIRE_HEADER (camfilter build_camfilter jpeglib.h stdio.h) + PLAYERDRIVER_ADD_DRIVER (camfilter build_camfilter LINKFLAGS "-ljpeg" SOURCES camfilter.cc) +ELSE (HAVE_JPEG) + PLAYERDRIVER_ADD_DRIVER (camfilter build_camfilter SOURCES camfilter.cc) +ENDIF (HAVE_JPEG) Added: code/player/trunk/server/drivers/camera/camfilter/COPYING =================================================================== --- code/player/trunk/server/drivers/camera/camfilter/COPYING (rev 0) +++ code/player/trunk/server/drivers/camera/camfilter/COPYING 2009-06-07 08:10:31 UTC (rev 7808) @@ -0,0 +1,340 @@ + GNU GENERAL PUBLIC LICENSE + Version 2, June 1991 + + Copyright (C) 1989, 1991 Free Software Foundation, Inc. + 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +License is intended to guarantee your freedom to share and change free +software--to make sure the software is free for all its users. This +General Public License applies to most of the Free Software +Foundation's software and to any other program whose authors commit to +using it. (Some other Free Software Foundation software is covered by +the GNU Library General Public License instead.) You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +this service if you wish), that you receive source code or can get it +if you want it, that you can change the software or use pieces of it +in new free programs; and that you know you can do these things. + + To protect your rights, we need to make restrictions that forbid +anyone to deny you these rights or to ask you to surrender the rights. +These restrictions translate to certain responsibilities for you if you +distribute copies of the software, or if you modify it. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must give the recipients all the rights that +you have. You must make sure that they, too, receive or can get the +source code. And you must show them these terms so they know their +rights. + + We protect your rights with two steps: (1) copyright the software, and +(2) offer you this license which gives you legal permission to copy, +distribute and/or modify the software. + + Also, for each author's protection and ours, we want to make certain +that everyone understands that there is no warranty for this free +software. If the software is modified by someone else and passed on, we +want its recipients to know that what they have is not the original, so +that any problems introduced by others will not reflect on the original +authors' reputations. + + Finally, any free program is threatened constantly by software +patents. We wish to avoid the danger that redistributors of a free +program will individually obtain patent licenses, in effect making the +program proprietary. To prevent this, we have made it clear that any +patent must be licensed for everyone's free use or not licensed at all. + + The precise terms and conditions for copying, distribution and +modification follow. + + GNU GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License applies to any program or other work which contains +a notice placed by the copyright holder saying it may be distributed +under the terms of this General Public License. The "Program", below, +refers to any such program or work, and a "work based on the Program" +means either the Program or any derivative work under copyright law: +that is to say, a work containing the Program or a portion of it, +either verbatim or with modifications and/or translated into another +language. (Hereinafter, translation is included without limitation in +the term "modification".) Each licensee is addressed as "you". + +Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running the Program is not restricted, and the output from the Program +is covered only if its contents constitute a work based on the +Program (independent of having been made by running the Program). +Whether that is true depends on what the Program does. + + 1. You may copy and distribute verbatim copies of the Program's +source code as you receive it, in any medium, provided that you +conspicuously and appropriately publish on each copy an appropriate +copyright notice and disclaimer of warranty; keep intact all the +notices that refer to this License and to the absence of any warranty; +and give any other recipients of the Program a copy of this License +along with the Program. + +You may charge a fee for the physical act of transferring a copy, and +you may at your option offer warranty protection in exchange for a fee. + + 2. You may modify your copy or copies of the Program or any portion +of it, thus forming a work based on the Program, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) You must cause the modified files to carry prominent notices + stating that you changed the files and the date of any change. + + b) You must cause any work that you distribute or publish, that in + whole or in part contains or is derived from the Program or any + part thereof, to be licensed as a whole at no charge to all third + parties under the terms of this License. + + c) If the modified program normally reads commands interactively + when run, you must cause it, when started running for such + interactive use in the most ordinary way, to print or display an + announcement including an appropriate copyright notice and a + notice that there is no warranty (or else, saying that you provide + a warranty) and that users may redistribute the program under + these conditions, and telling the user how to view a copy of this + License. (Exception: if the Program itself is interactive but + does not normally print such an announcement, your work based on + the Program is not required to print an announcement.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Program, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Program, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Program. + +In addition, mere aggregation of another work not based on the Program +with the Program (or with a work based on the Program) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may copy and distribute the Program (or a work based on it, +under Section 2) in object code or executable form under the terms of +Sections 1 and 2 above provided that you also do one of the following: + + a) Accompany it with the complete corresponding machine-readable + source code, which must be distributed under the terms of Sections + 1 and 2 above on a medium customarily used for software interchange; or, + + b) Accompany it with a written offer, valid for at least three + years, to give any third party, for a charge no more than your + cost of physically performing source distribution, a complete + machine-readable copy of the corresponding source code, to be + distributed under the terms of Sections 1 and 2 above on a medium + customarily used for software interchange; or, + + c) Accompany it with the information you received as to the offer + to distribute corresponding source code. (This alternative is + allowed only for noncommercial distribution and only if you + received the program in object code or executable form with such + an offer, in accord with Subsection b above.) + +The source code for a work means the preferred form of the work for +making modifications to it. For an executable work, complete source +code means all the source code for all modules it contains, plus any +associated interface definition files, plus the scripts used to +control compilation and installation of the executable. However, as a +special exception, the source code distributed need not include +anything that is normally distributed (in either source or binary +form) with the major components (compiler, kernel, and so on) of the +operating system on which the executable runs, unless that component +itself accompanies the executable. + +If distribution of executable or object code is made by offering +access to copy from a designated place, then offering equivalent +access to copy the source code from the same place counts as +distribution of the source code, even though third parties are not +compelled to copy the source along with the object code. + + 4. You may not copy, modify, sublicense, or distribute the Program +except as expressly provided under this License. Any attempt +otherwise to copy, modify, sublicense or distribute the Program is +void, and will automatically terminate your rights under this License. +However, parties who have received copies, or rights, from you under +this License will not have their licenses terminated so long as such +parties remain in full compliance. + + 5. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Program or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Program (or any work based on the +Program), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Program or works based on it. + + 6. Each time you redistribute the Program (or any work based on the +Program), the recipient automatically receives a license from the +original licensor to copy, distribute or modify the Program subject to +these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties to +this License. + + 7. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Program at all. For example, if a patent +license would not permit royalty-free redistribution of the Program by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Program. + +If any portion of this section is held invalid or unenforceable under +any particular circumstance, the balance of the section is intended to +apply and the section as a whole is intended to apply in other +circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system, which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 8. If the distribution and/or use of the Program is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Program under this License +may add an explicit geographical distribution limitation excluding +those countries, so that distribution is permitted only in or among +countries not thus excluded. In such case, this License incorporates +the limitation as if written in the body of this License. + + 9. The Free Software Foundation may publish revised and/or new versions +of the General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + +Each version is given a distinguishing version number. If the Program +specifies a version number of this License which applies to it and "any +later version", you have the option of following the terms and conditions +either of that version or of any later version published by the Free +Software Foundation. If the Program does not specify a version number of +this License, you may choose any version ever published by the Free Software +Foundation. + + 10. If you wish to incorporate parts of the Program into other free +programs whose distribution conditions are different, write to the author +to ask for permission. For software which is copyrighted by the Free +Software Foundation, write to the Free Software Foundation; we sometimes +make exceptions for this. Our decision will be guided by the two goals +of preserving the free status of all derivatives of our free software and +of promoting the sharing and reuse of software generally. + + NO WARRANTY + + 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY +FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN +OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES +PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED +OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS +TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE +PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, +REPAIR OR CORRECTION. + + 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR +REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, +INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING +OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED +TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY +YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER +PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE +POSSIBILITY OF SUCH DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + <one line to give the program's name and a brief idea of what it does.> + Copyright (C) 19yy <name of author> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + + +Also add information on how to contact you by electronic and paper mail. + +If the program is interactive, make it output a short notice like this +when it starts in an interactive mode: + + Gnomovision version 69, Copyright (C) 19yy name of author + Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, the commands you use may +be called something other than `show w' and `show c'; they could even be +mouse-clicks or menu items--whatever suits your program. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the program, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the program + `Gnomovision' (which makes passes at compilers) written by James Hacker. + + <signature of Ty Coon>, 1 April 1989 + Ty Coon, President of Vice + +This General Public License does not permit incorporating your program into +proprietary programs. If your program is a subroutine library, you may +consider it more useful to permit linking proprietary applications with the +library. If this is what you want to do, use the GNU Library General +Public License instead of this License. Added: code/player/trunk/server/drivers/camera/camfilter/camfilter.cc =================================================================== --- code/player/trunk/server/drivers/camera/camfilter/camfilter.cc (rev 0) +++ code/player/trunk/server/drivers/camera/camfilter/camfilter.cc 2009-06-07 08:10:31 UTC (rev 7808) @@ -0,0 +1,524 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey et al. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +///////////////////////////////////////////////////////////////////////////// +// +// Desc: Multiband thresholding for camera interface, +// see http://en.wikipedia.org/wiki/Thresholding_%28image_processing%29 +// Author: Paul Osmialowski +// Date: 24 Jun 2008 +// +///////////////////////////////////////////////////////////////////////////// + +/** @ingroup drivers */ +/** @{ */ +/** @defgroup driver_camfilter camfilter + * @brief camera image filtering driver + +The camfilter driver filters colors of pixels on whole given camera image. + +@par Compile-time dependencies + +- none + +@par Provides + +- @ref interface_camera + +@par Requires + +- @ref interface_camera + +@par Configuration requests + +- none + +@par Configuration file options + +- max_color_only (integer) + - Default: 0 (no effect) + - when set to 1, only max from R G B is passed, other values are changed to 0 + +- r_min (integer) + - Default: -1 (no filter) + - R minimal threshold value +- g_min (integer) + - Default: -1 (no filter) + - G minimal threshold value +- b_min (integer) + - Default: -1 (no filter) + - B minimal threshold value +- grey_min (integer) + - Default: -1 (no filter) + - GREY minimal threshold value + +- r_max (integer) + - Default: -1 (no filter) + - R maximal threshold value +- g_max (integer) + - Default: -1 (no filter) + - G maximal threshold value +- b_max (integer) + - Default: -1 (no filter) + - B maximal threshold value +- grey_max (integer) + - Default: -1 (no filter) + - GREY maximal threshold value + +- r_above (integer) + - Default: 255 + - new value for each R value above the R maximal threshold +- g_above (integer) + - Default: 255 + - new value for each G value above the G maximal threshold +- b_above (integer) + - Default: 255 + - new value for each B value above the B maximal threshold +- grey_above (integer) + - Default: 255 + - new value for each RGB value above the GREY maximal threshold + +- r_below (integer) + - Default: 0 + - new value for each R value below the R minimal threshold +- g_below (integer) + - Default: 0 + - new value for each G value below the G minimal threshold +- b_below (integer) + - Default: 0 + - new value for each B value below the B minimal threshold +- grey_below (integer) + - Default: 0 + - new value for each RGB value below the GREY minimal threshold + +- r_passed (integer) + - Default: -1 (no change) + - new value for each R value between the R minimal and the R maximal thresholds +- g_passed (integer) + - Default: -1 (no change) + - new value for each G value between the G minimal and the G maximal thresholds +- b_passed (integer) + - Default: -1 (no change) + - new value for each B value between the B minimal and the B maximal thresholds +- grey_passed (integer) + - Default: -1 (no change) + - new value for each RGB value between the GREY minimal and the GREY maximal thresholds (-1 = no change, -2 = RGB->GREY conversion) + - this setting overrides other *_passed settings + +@par Example + +@verbatim +driver +( + name "camfilter" + requires ["camera:1"] + provides ["camera:0"] + r_min 128 + r_below 0 + r_passed 255 + g_passed 0 + b_passed 0 +) +@endverbatim + +@author Paul Osmialowski + +*/ +/** @} */ + +#include <stddef.h> +#include <stdlib.h> +#include <string.h> +#include <assert.h> +#include <pthread.h> +#include <libplayercore/playercore.h> +#if HAVE_JPEGLIB_H +#include <libplayerjpeg/playerjpeg.h> +#else +#warning JPEG decompression disabled! +#endif + +class CamFilter : public ThreadedDriver +{ + public: CamFilter(ConfigFile * cf, int section); + public: virtual ~CamFilter(); + + public: virtual int MainSetup(); + public: virtual void MainQuit(); + + // This method will be invoked on each incoming message + public: virtual int ProcessMessage(QueuePointer & resp_queue, + player_msghdr * hdr, + void * data); + + private: virtual void Main(); + + // Input camera device + private: player_devaddr_t camera_provided_addr, camera_id; + private: Device * camera; + private: unsigned char * buffer; + private: size_t bufsize; + + private: int max_color_only; + private: int r_min; + private: int g_min; + private: int b_min; + private: int grey_min; + private: int r_max; + private: int g_max; + private: int b_max; + private: int grey_max; + private: int r_above; + private: int g_above; + private: int b_above; + private: int grey_above; + private: int r_below; + private: int g_below; + private: int b_below; + private: int grey_below; + private: int r_passed; + private: int g_passed; + private: int b_passed; + private: int grey_passed; +}; + +Driver * CamFilter_Init(ConfigFile * cf, int section) +{ + return reinterpret_cast<Driver *>(new CamFilter(cf, section)); +} + +void camfilter_Register(DriverTable *table) +{ + table->AddDriver("camfilter", CamFilter_Init); +} + +CamFilter::CamFilter(ConfigFile * cf, int section) + : ThreadedDriver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN) +{ + memset(&(this->camera_provided_addr), 0, sizeof(player_devaddr_t)); + memset(&(this->camera_id), 0, sizeof(player_devaddr_t)); + this->camera = NULL; + this->buffer = NULL; + this->bufsize = 0; + if (cf->ReadDeviceAddr(&(this->camera_provided_addr), section, "provides", PLAYER_CAMERA_CODE, -1, NULL)) + { + this->SetError(-1); + return; + } + if (this->AddInterface(this->camera_provided_addr)) + { + this->SetError(-1); + return; + } + if (cf->ReadDeviceAddr(&this->camera_id, section, "requires", PLAYER_CAMERA_CODE, -1, NULL) != 0) + { + this->SetError(-1); + return; + } + this->max_color_only = cf->ReadInt(section, "max_color_only", 0); + this->r_min = cf->ReadInt(section, "r_min", -1); + this->g_min = cf->ReadInt(section, "g_min", -1); + this->b_min = cf->ReadInt(section, "b_min", -1); + this->grey_min = cf->ReadInt(section, "grey_min", -1); + this->r_max = cf->ReadInt(section, "r_max", -1); + this->g_max = cf->ReadInt(section, "g_max", -1); + this->b_max = cf->ReadInt(section, "b_max", -1); + this->grey_max = cf->ReadInt(section, "grey_max", -1); + this->r_above = cf->ReadInt(section, "r_above", 255); + this->g_above = cf->ReadInt(section, "g_above", 255); + this->b_above = cf->ReadInt(section, "b_above", 255); + this->grey_above = cf->ReadInt(section, "grey_above", 255); + this->r_below = cf->ReadInt(section, "r_below", 0); + this->g_below = cf->ReadInt(section, "g_below", 0); + this->b_below = cf->ReadInt(section, "b_below", 0); + this->grey_below = cf->ReadInt(section, "grey_below", 0); + this->r_passed = cf->ReadInt(section, "r_passed", -1); + this->g_passed = cf->ReadInt(section, "g_passed", -1); + this->b_passed = cf->ReadInt(section, "b_passed", -1); + this->grey_passed = cf->ReadInt(section, "grey_passed", -1); +} + +CamFilter::~CamFilter() +{ + if (this->buffer) free(this->buffer); +} + +int CamFilter::MainSetup() +{ + if (Device::MatchDeviceAddress(this->camera_id, this->camera_provided_addr)) + { + PLAYER_ERROR("attempt to subscribe to self"); + return -1; + } + this->camera = deviceTable->GetDevice(this->camera_id); + if (!this->camera) + { + PLAYER_ERROR("unable to locate suitable camera device"); + return -1; + } + if (this->camera->Subscribe(this->InQueue) != 0) + { + PLAYER_ERROR("unable to subscribe to camera device"); + this->camera = NULL; + return -1; + } + return 0; +} + +void CamFilter::MainQuit() +{ + if (this->camera) this->camera->Unsubscribe(this->InQueue); + this->camera = NULL; +} + +void CamFilter::Main() +{ + for (;;) + { + this->InQueue->Wait(); + pthread_testcancel(); + this->ProcessMessages(); + pthread_testcancel(); + } +} + +int CamFilter::ProcessMessage(QueuePointer & resp_queue, player_msghdr * hdr, void * data) +{ + player_camera_data_t * output; + int i, j; + size_t new_size; + unsigned char * ptr, * ptr1; + player_camera_data_t * rawdata; + unsigned char r, g, b, grey, max; + + assert(hdr); + if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA, PLAYER_CAMERA_DATA_STATE, this->camera_id)) + { + assert(data); + rawdata = reinterpret_cast<player_camera_data_t *>(data); + if ((rawdata->width <= 0) || (rawdata->height <= 0)) + { + return -1; + } else + { + new_size = rawdata->width * rawdata->height * 3; + ptr = NULL; + switch (rawdata->compression) + { + case PLAYER_CAMERA_COMPRESS_RAW: + switch (rawdata->bpp) + { + case 8: + if (this->bufsize != new_size) + { + if (this->buffer) free(this->buffer); + this->buffer = NULL; + this->bufsize = 0; + } + if (!(this->buffer)) + { + this->bufsize = 0; + this->buffer = reinterpret_cast<unsigned char *>(malloc(new_size)); + if (!(this->buffer)) + { + PLAYER_ERROR("Out of memory"); + return -1; + } + this->bufsize = new_size; + } + ptr = this->buffer; + ptr1 = reinterpret_cast<unsigned char *>(rawdata->image); + for (i = 0; i < static_cast<int>(rawdata->height); i++) + { + for (j = 0; j < static_cast<int>(rawdata->width); j++) + { + ptr[0] = *ptr1; + ptr[1] = *ptr1; + ptr[2] = *ptr1; + ptr += 3; ptr1++; + } + } + ptr = this->buffer; + break; + case 24: + ptr = reinterpret_cast<unsigned char *>(rawdata->image); + break; + case 32: + if (this->bufsize != new_size) + { + if (this->buffer) free(this->buffer); + this->buffer = NULL; + this->bufsize = 0; + } + if (!(this->buffer)) + { + this->bufsize = 0; + this->buffer = reinterpret_cast<unsigned char *>(malloc(new_size)); + if (!(this->buffer)) + { + PLAYER_ERROR("Out of memory"); + return -1; + } + this->bufsize = new_size; + } + ptr = this->buffer; + ptr1 = reinterpret_cast<unsigned char *>(rawdata->image); + for (i = 0; i < static_cast<int>(rawdata->height); i++) + { + for (j = 0; j < static_cast<int>(rawdata->width); j++) + { + ptr[0] = ptr1[0]; + ptr[1] = ptr1[1]; + ptr[2] = ptr1[2]; + ptr += 3; ptr1 += 4; + } + } + ptr = this->buffer; + break; + default: + PLAYER_WARN("unsupported image depth (not good)"); + return -1; + } + break; +#if HAVE_JPEGLIB_H + case PLAYER_CAMERA_COMPRESS_JPEG: + if (this->bufsize != new_size) + { + if (this->buffer) free(this->buffer); + this->buffer = NULL; + this->bufsize = 0; + } + if (!(this->buffer)) + { + this->bufsize = 0; + this->buffer = reinterpret_cast<unsigned char *>(malloc(new_size)); + if (!(this->buffer)) + { + PLAYER_ERROR("Out of memory"); + return -1; + } + this->bufsize = new_size; + } + jpeg_decompress(this->buffer, this->bufsize, rawdata->image, rawdata->image_count); + ptr = this->buffer; + break; +#endif + default: + PLAYER_WARN("unsupported compression scheme (not good)"); + return -1; + } + assert(ptr); + output = reinterpret_cast<player_camera_data_t *>(malloc(sizeof(player_camera_data_t))); + if (!output) + { + PLAYER_ERROR("Out of memory"); + return -1; + } + memset(output, 0, sizeof(player_camera_data_t)); + output->bpp = 24; + output->format = PLAYER_CAMERA_FORMAT_RGB888; + output->fdiv = rawdata->fdiv; + output->width = rawdata->width; + output->height = rawdata->height; + output->image_count = output->width * output->height * 3; + output->image = reinterpret_cast<uint8_t *>(malloc(output->image_count)); + if (!(output->image)) + { + free(output); + PLAYER_ERROR("Out of memory"); + return -1; + } + ptr1 = ptr; + ptr = reinterpret_cast<unsigned char *>(output->image); + for (i = 0; i < static_cast<int>(rawdata->height); i++) + { + for (j = 0; j < static_cast<int>(rawdata->width); j++) + { + r = ptr1[0]; + g = ptr1[1]; + b = ptr1[2]; + if (this->max_color_only) + { + max = r; + if (g > max) max = g; + if (b > max) max = b; + if (r < max) r = 0; + if (g < max) g = 0; + if (b < max) b = 0; + } + grey = static_cast<unsigned char>((0.299 * static_cast<double>(r)) + (0.587 * static_cast<double>(g)) + (0.114 * static_cast<double>(b))); + switch (this->grey_passed) + { + case -1: + ptr[0] = (this->r_passed != -1) ? static_cast<unsigned char>(this->r_passed) : r; + ptr[1] = (this->g_passed != -1) ? static_cast<unsigned char>(this->g_passed) : g; + ptr[2] = (this->b_passed != -1) ? static_cast<unsigned char>(this->b_passed) : b; + break; + case -2: + ptr[0] = grey; + ptr[1] = grey; + ptr[2] = grey; + break; + default: + ptr[0] = static_cast<unsigned char>(this->grey_passed); + ptr[1] = static_cast<unsigned char>(this->grey_passed); + ptr[2] = static_cast<unsigned char>(this->grey_passed); + } + if ((this->r_min >= 0) && (r < static_cast<unsigned char>(this->r_min))) ptr[0] = static_cast<unsigned char>(this->r_below); + if ((this->g_min >= 0) && (g < static_cast<unsigned char>(this->g_min))) ptr[1] = static_cast<unsigned char>(this->g_below); + if ((this->b_min >= 0) && (b < static_cast<unsigned char>(this->b_min))) ptr[2] = static_cast<unsigned char>(this->b_below); + if ((this->grey_min >= 0) && (grey < static_cast<unsigned char>(this->grey_min))) + { + ptr[0] = static_cast<unsigned char>(this->grey_below); + ptr[1] = static_cast<unsigned char>(this->grey_below); + ptr[2] = static_cast<unsigned char>(this->grey_below); + } + if ((this->r_max >= 0) && (r > static_cast<unsigned char>(this->r_max))) ptr[0] = static_cast<unsigned char>(this->r_above); + if ((this->g_max >= 0) && (g > static_cast<unsigned char>(this->g_max))) ptr[1] = static_cast<unsigned char>(this->g_above); + if ((this->b_max >= 0) && (b > static_cast<unsigned char>(this->b_max))) ptr[2] = static_cast<unsigned char>(this->b_above); + if ((this->grey_max >= 0) && (grey > static_cast<unsigned char>(this->grey_max))) + { + ptr[0] = static_cast<unsigned char>(this->grey_above); + ptr[1] = static_cast<unsigned char>(this->grey_above); + ptr[2] = static_cast<unsigned char>(this->grey_above); + } + ptr += 3; ptr1 += 3; + } + } + Publish(this->camera_provided_addr, PLAYER_MSGTYPE_DATA, PLAYER_CAMERA_DATA_STATE, reinterpret_cast<void *>(output), 0, &(hdr->timestamp), false); + // I assume that Publish() freed those output data! + output = NULL; + } + return 0; + } + return -1; +} + +#ifdef JUST_A_PLUGIN + +extern "C" +{ + int player_driver_init(DriverTable * table) + { + CamFilter_Register(table); + return 0; + } +} + +#endif Added: code/player/trunk/server/drivers/camera/camfilter/version.txt =================================================================== --- code/player/trunk/server/drivers/camera/camfilter/version.txt (rev 0) +++ code/player/trunk/server/drivers/camera/camfilter/version.txt 2009-06-07 08:10:31 UTC (rev 7808) @@ -0,0 +1 @@ +20090315 Added: code/player/trunk/server/drivers/dio/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/dio/CMakeLists.txt (rev 0) +++ code/player/trunk/server/drivers/dio/CMakeLists.txt 2009-06-07 08:10:31 UTC (rev 7808) @@ -0,0 +1,2 @@ +ADD_SUBDIRECTORY (serio) + Added: code/player/trunk/server/drivers/dio/serio/AUTHORS =================================================================== --- code/player/trunk/server/drivers/dio/serio/AUTHORS (rev 0) +++ code/player/trunk/server/drivers/dio/serio/AUTHORS 2009-06-07 08:10:31 UTC (rev 7808) @@ -0,0 +1 @@ +Paul Osmialowski <new...@ki...> Added: code/player/trunk/server/drivers/dio/serio/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/dio/serio/CMakeLists.txt (rev 0) +++ code/player/trunk/server/drivers/dio/serio/CMakeLists.txt 2009-06-07 08:10:31 UTC (rev 7808) @@ -0,0 +1,3 @@ +PLAYERDRIVER_OPTION (serio build_serio ON) +PLAYERDRIVER_REJECT_OS (serio build_serio PLAYER_OS_WIN) +PLAYERDRIVER_ADD_DRIVER (serio build_serio SOURCES serio.cc) Added: code/player/trunk/server/drivers/dio/serio/COPYING =================================================================== --- code/player/trunk/server/drivers/dio/serio/COPYING (rev 0) +++ code/player/trunk/server/drivers/dio/serio/COPYING 2009-06-07 08:10:31 UTC (rev 7808) @@ -0,0 +1,340 @@ + GNU GENERAL PUBLIC LICENSE + Version 2, June 1991 + + Copyright (C) 1989, 1991 Free Software Foundation, Inc. + 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +License is intended to guarantee your freedom to share and change free +software--to make sure the software is free for all its users. This +General Public License applies to most of the Free Software +Foundation's software and to any other program whose authors commit to +using it. (Some other Free Software Foundation software is covered by +the GNU Library General Public License instead.) You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +this service if you wish), that you receive source code or can get it +if you want it, that you can change the software or use pieces of it +in new free programs; and that you know you can do these things. + + To protect your rights, we need to make restrictions that forbid +anyone to deny you these rights or to ask you to surrender the rights. +These restrictions translate to certain responsibilities for you if you +distribute copies of the software, or if you modify it. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must give the recipients all the rights that +you have. You must make sure that they, too, receive or can get the +source code. And you must show them these terms so they know their +rights. + + We protect your rights with two steps: (1) copyright the software, and +(2) offer you this license which gives you legal permission to copy, +distribute and/or modify the software. + + Also, for each author's protection and ours, we want to make certain +that everyone understands that there is no warranty for this free +software. If the software is modified by someone else and passed on, we +want its recipients to know that what they have is not the original, so +that any problems introduced by others will not reflect on the original +authors' reputations. + + Finally, any free program is threatened constantly by software +patents. We wish to avoid the danger that redistributors of a free +program will individually obtain patent licenses, in effect making the +program proprietary. To prevent this, we have made it clear that any +patent must be licensed for everyone's free use or not licensed at all. + + The precise terms and conditions for copying, distribution and +modification follow. + + GNU GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License applies to any program or other work which contains +a notice placed by the copyright holder saying it may be distributed +under the terms of this General Public License. The "Program", below, +refers to any such program or work, and a "work based on the Program" +means either the Program or any derivative work under copyright law: +that is to say, a work containing the Program or a portion of it, +either verbatim or with modifications and/or translated into another +language. (Hereinafter, translation is included without limitation in +the term "modification".) Each licensee is addressed as "you". + +Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running the Program is not restricted, and the output from the Program +is covered only if its contents constitute a work based on the +Program (independent of having been made by running the Program). +Whether that is true depends on what the Program does. + + 1. You may copy and distribute verbatim copies of the Program's +source code as you receive it, in any medium, provided that you +conspicuously and appropriately publish on each copy an appropriate +copyright notice and disclaimer of warranty; keep intact all the +notices that refer to this License and to the absence of any warranty; +and give any other recipients of the Program a copy of this License +along with the Program. + +You may charge a fee for the physical act of transferring a copy, and +you may at your option offer warranty protection in exchange for a fee. + + 2. You may modify your copy or copies of the Program or any portion +of it, thus forming a work based on the Program, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) You must cause the modified files to carry prominent notices + stating that you changed the files and the date of any change. + + b) You must cause any work that you distribute or publish, that in + whole or in part contains or is derived from the Program or any + part thereof, to be licensed as a whole at no charge to all third + parties under the terms of this License. + + c) If the modified program normally reads commands interactively + when run, you must cause it, when started running for such + interactive use in the most ordinary way, to print or display an + announcement including an appropriate copyright notice and a + notice that there is no warranty (or else, saying that you provide + a warranty) and that users may redistribute the program under + these conditions, and telling the user how to view a copy of this + License. (Exception: if the Program itself is interactive but + does not normally print such an announcement, your work based on + the Program is not required to print an announcement.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Program, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Program, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Program. + +In addition, mere aggregation of another work not based on the Program +with the Program (or with a work based on the Program) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may copy and distribute the Program (or a work based on it, +under Section 2) in object code or executable form under the terms of +Sections 1 and 2 above provided that you also do one of the following: + + a) Accompany it with the complete corresponding machine-readable + source code, which must be distributed under the terms of Sections + 1 and 2 above on a medium customarily used for software interchange; or, + + b) Accompany it with a written offer, valid for at least three + years, to give any third party, for a charge no more than your + cost of physically performing source distribution, a complete + machine-readable copy of the corresponding source code, to be + distributed under the terms of Sections 1 and 2 above on a medium + customarily used for software interchange; or, + + c) Accompany it with the information you received as to the offer + to distribute corresponding source code. (This alternative is + allowed only for noncommercial distribution and only if you + received the program in object code or executable form with such + an offer, in accord with Subsection b above.) + +The source code for a work means the preferred form of the work for +making modifications to it. For an executable work, complete source +code means all the source code for all modules it contains, plus any +associated interface definition files, plus the scripts used to +control compilation and installation of the executable. However, as a +special exception, the source code distributed need not include +anything that is normally distributed (in either source or binary +form) with the major components (compiler, kernel, and so on) of the +operating system on which the executable runs, unless that component +itself accompanies the executable. + +If distribution of executable or object code is made by offering +access to copy from a designated place, then offering equivalent +access to copy the source code from the same place counts as +distribution of the source code, even though third parties are not +compelled to copy the source along with the object code. + + 4. You may not copy, modify, sublicense, or distribute the Program +except as expressly provided under this License. Any attempt +otherwise to copy, modify, sublicense or distribute the Program is +void, and will automatically terminate your rights under this License. +However, parties who have received copies, or rights, from you under +this License will not have their licenses terminated so long as such +parties remain in full compliance. + + 5. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Program or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Program (or any work based on the +Program), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Program or works based on it. + + 6. Each time you redistribute the Program (or any work based on the +Program), the recipient automatically receives a license from the +original licensor to copy, distribute or modify the Program subject to +these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties to +this License. + + 7. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Program at all. For example, if a patent +license would not permit royalty-free redistribution of the Program by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Program. + +If any portion of this section is held invalid or unenforceable under +any particular circumstance, the balance of the section is intended to +apply and the section as a whole is intended to apply in other +circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system, which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 8. If the distribution and/or use of the Program is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Program under this License +may add an explicit geographical distribution limitation excluding +those countries, so that distribution is permitted only in or among +countries not thus excluded. In such case, this License incorporates +the limitation as if written in the body of this License. + + 9. The Free Software Foundation may publish revised and/or new versions +of the General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + +Each version is given a distinguishing version number. If the Program +specifies a version number of this License which applies to it and "any +later version", you have the option of following the terms and conditions +either of that version or of any later version published by the Free +Software Foundation. If the Program does not specify a version number of +this License, you may choose any version ever published by the Free Software +Foundation. + + 10. If you wish to incorporate parts of the Program into other free +programs whose distribution conditions are different, write to the author +to ask for permission. For software which is copyrighted by the Free +Software Foundation, write to the Free Software Foundation; we sometimes +make exceptions for this. Our decision will be guided by the two goals +of preserving the free status of all derivatives of our free software and +of promoting the sharing and reuse of software generally. + + NO WARRANTY + + 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY +FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN +OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES +PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED +OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS +TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE +PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, +REPAIR OR CORRECTION. + + 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR +REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, +INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING +OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED +TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY +YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER +PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE +POSSIBILITY OF SUCH DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + <one line to give the program's name and a brief idea of what it does.> + Copyright (C) 19yy <name of author> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + + +Also add information on how to contact you by electronic and paper mail. + +If the program is interactive, make it output a short notice like this +when it starts in an interactive mode: + + Gnomovision version 69, Copyright (C) 19yy name of author + Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, the commands you use may +be called something other than `show w' and `show c'; they could even be +mouse-clicks or menu items--whatever suits your program. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the program, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the program + `Gnomovision' (which makes passes at compilers) written by James Hacker. + + <signature of Ty Coon>, 1 April 1989 + Ty Coon, President of Vice + +This General Public License does not permit incorporating your program into +proprietary programs. If your program is a subroutine library, you may +consider it more useful to permit linking proprietary applications with the +library. If this is what you want to do, use the GNU Library General +Public License instead of this License. Added: code/player/trunk/server/drivers/dio/serio/serio.cc =================================================================== --- code/player/trunk/server/drivers/dio/serio/serio.cc (rev 0) +++ code/player/trunk/server/drivers/dio/serio/serio.cc 2009-06-07 08:10:31 UTC (rev 7808) @@ -0,0 +1,277 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey et al. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +///////////////////////////////////////////////////////////////////////////// +// +// Desc: Low-level access to the serial port control lines +// Author: Paul Osmialowski +// Date: 22 Aug 2008 +// +///////////////////////////////////////////////////////////////////////////// + +/** @ingroup drivers */ +/** @{ */ +/** @defgroup driver_serio serio + * @brief low-level access to the serial port control lines + +The serio driver provides low-level access to the serial port control lines. + +The digout bitfield of dio interface command structure needs lowest two bits +to be filled: +0 - new RTS line state (output line) +1 - new DTR line state (output line) + +This driver provides data by filling bits bitfield of dio interface data +structure. +Lowest six bits are filled by the driver: +0 - current RTS line state (output line) +1 - current DTR line state (output line) +2 - currend DCD line state (input line) +3 - current CTS line state (input line) +4 - current DSR line state (input line) +5 - current RI line state (input line) + +@par Compile-time dependencies + +- none + +@par Provides + +- @ref interface_dio + +@par Requires + +- none + +@par Configuration requests + +- none + +@par Configurat... [truncated message content] |
From: <gb...@us...> - 2009-06-08 06:31:43
|
Revision: 7815 http://playerstage.svn.sourceforge.net/playerstage/?rev=7815&view=rev Author: gbiggs Date: 2009-06-08 06:31:35 +0000 (Mon, 08 Jun 2009) Log Message: ----------- Applied patch #2802472 Modified Paths: -------------- code/player/trunk/server/drivers/camera/1394/camera1394.cc code/player/trunk/server/drivers/camera/camfilter/camfilter.cc Removed Paths: ------------- code/player/trunk/server/drivers/camera/camfilter/AUTHORS code/player/trunk/server/drivers/camera/camfilter/COPYING code/player/trunk/server/drivers/camera/camfilter/version.txt code/player/trunk/server/drivers/dio/serio/AUTHORS code/player/trunk/server/drivers/dio/serio/COPYING code/player/trunk/server/drivers/dio/serio/version.txt Modified: code/player/trunk/server/drivers/camera/1394/camera1394.cc =================================================================== --- code/player/trunk/server/drivers/camera/1394/camera1394.cc 2009-06-08 06:20:28 UTC (rev 7814) +++ code/player/trunk/server/drivers/camera/1394/camera1394.cc 2009-06-08 06:31:35 UTC (rev 7815) @@ -914,7 +914,7 @@ if (DC1394_SUCCESS != dc1394_feature_set_value(this->camera, FEATURE_GAIN,gain)) #else if (DC1394_SUCCESS != dc1394_set_gain(this->handle, this->camera.node, - this->gain)) + gain)) #endif { PLAYER_ERROR("Unable to Gain value"); @@ -1000,45 +1000,45 @@ #endif // apply user config file provided camera settings - if (Focus != PROPERTY_NOT_SET) { - if (SetFocus(Focus)) { + if (this->Focus != PROPERTY_NOT_SET) { + if (this->SetFocus(this->Focus)) { this->SafeCleanup(); return -1; } } - if (Iris != PROPERTY_NOT_SET) { - if (SetIris(Iris)) { + if (this->Iris != PROPERTY_NOT_SET) { + if (this->SetIris(this->Iris)) { // error this->SafeCleanup(); return -1; } } - if (Brightness != PROPERTY_NOT_SET) { - if (SetBrightness(Brightness)) { + if (this->Brightness != PROPERTY_NOT_SET) { + if (this->SetBrightness(this->Brightness)) { this->SafeCleanup(); return -1; } } - if (Exposure != PROPERTY_NOT_SET) { - if (SetExposure(Exposure)) { + if (this->Exposure != PROPERTY_NOT_SET) { + if (this->SetExposure(this->Exposure)) { this->SafeCleanup(); return -1; } } - if (Shutter != PROPERTY_NOT_SET) { - if (SetShutter(Shutter)) { + if (this->Shutter != PROPERTY_NOT_SET) { + if (this->SetShutter(this->Shutter)) { this->SafeCleanup(); return -1; } } - if (Gain != PROPERTY_NOT_SET) { - if (SetGain(Gain)) { + if (this->Gain != PROPERTY_NOT_SET) { + if (this->SetGain(this->Gain)) { this->SafeCleanup(); return -1; } } - if (BlueBalance != PROPERTY_NOT_SET && RedBalance != PROPERTY_NOT_SET) { + if ((this->BlueBalance != PROPERTY_NOT_SET) && (this->RedBalance != PROPERTY_NOT_SET)) { #if LIBDC1394_VERSION == 0200 if (DC1394_SUCCESS != dc1394_feature_whitebalance_set_value(this->camera,this->BlueBalance,this->RedBalance)) #else Deleted: code/player/trunk/server/drivers/camera/camfilter/AUTHORS =================================================================== --- code/player/trunk/server/drivers/camera/camfilter/AUTHORS 2009-06-08 06:20:28 UTC (rev 7814) +++ code/player/trunk/server/drivers/camera/camfilter/AUTHORS 2009-06-08 06:31:35 UTC (rev 7815) @@ -1 +0,0 @@ -Paul Osmialowski <new...@ki...> Deleted: code/player/trunk/server/drivers/camera/camfilter/COPYING =================================================================== --- code/player/trunk/server/drivers/camera/camfilter/COPYING 2009-06-08 06:20:28 UTC (rev 7814) +++ code/player/trunk/server/drivers/camera/camfilter/COPYING 2009-06-08 06:31:35 UTC (rev 7815) @@ -1,340 +0,0 @@ - GNU GENERAL PUBLIC LICENSE - Version 2, June 1991 - - Copyright (C) 1989, 1991 Free Software Foundation, Inc. - 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - Everyone is permitted to copy and distribute verbatim copies - of this license document, but changing it is not allowed. - - Preamble - - The licenses for most software are designed to take away your -freedom to share and change it. By contrast, the GNU General Public -License is intended to guarantee your freedom to share and change free -software--to make sure the software is free for all its users. This -General Public License applies to most of the Free Software -Foundation's software and to any other program whose authors commit to -using it. (Some other Free Software Foundation software is covered by -the GNU Library General Public License instead.) You can apply it to -your programs, too. - - When we speak of free software, we are referring to freedom, not -price. Our General Public Licenses are designed to make sure that you -have the freedom to distribute copies of free software (and charge for -this service if you wish), that you receive source code or can get it -if you want it, that you can change the software or use pieces of it -in new free programs; and that you know you can do these things. - - To protect your rights, we need to make restrictions that forbid -anyone to deny you these rights or to ask you to surrender the rights. -These restrictions translate to certain responsibilities for you if you -distribute copies of the software, or if you modify it. - - For example, if you distribute copies of such a program, whether -gratis or for a fee, you must give the recipients all the rights that -you have. You must make sure that they, too, receive or can get the -source code. And you must show them these terms so they know their -rights. - - We protect your rights with two steps: (1) copyright the software, and -(2) offer you this license which gives you legal permission to copy, -distribute and/or modify the software. - - Also, for each author's protection and ours, we want to make certain -that everyone understands that there is no warranty for this free -software. If the software is modified by someone else and passed on, we -want its recipients to know that what they have is not the original, so -that any problems introduced by others will not reflect on the original -authors' reputations. - - Finally, any free program is threatened constantly by software -patents. We wish to avoid the danger that redistributors of a free -program will individually obtain patent licenses, in effect making the -program proprietary. To prevent this, we have made it clear that any -patent must be licensed for everyone's free use or not licensed at all. - - The precise terms and conditions for copying, distribution and -modification follow. - - GNU GENERAL PUBLIC LICENSE - TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION - - 0. This License applies to any program or other work which contains -a notice placed by the copyright holder saying it may be distributed -under the terms of this General Public License. The "Program", below, -refers to any such program or work, and a "work based on the Program" -means either the Program or any derivative work under copyright law: -that is to say, a work containing the Program or a portion of it, -either verbatim or with modifications and/or translated into another -language. (Hereinafter, translation is included without limitation in -the term "modification".) Each licensee is addressed as "you". - -Activities other than copying, distribution and modification are not -covered by this License; they are outside its scope. The act of -running the Program is not restricted, and the output from the Program -is covered only if its contents constitute a work based on the -Program (independent of having been made by running the Program). -Whether that is true depends on what the Program does. - - 1. You may copy and distribute verbatim copies of the Program's -source code as you receive it, in any medium, provided that you -conspicuously and appropriately publish on each copy an appropriate -copyright notice and disclaimer of warranty; keep intact all the -notices that refer to this License and to the absence of any warranty; -and give any other recipients of the Program a copy of this License -along with the Program. - -You may charge a fee for the physical act of transferring a copy, and -you may at your option offer warranty protection in exchange for a fee. - - 2. You may modify your copy or copies of the Program or any portion -of it, thus forming a work based on the Program, and copy and -distribute such modifications or work under the terms of Section 1 -above, provided that you also meet all of these conditions: - - a) You must cause the modified files to carry prominent notices - stating that you changed the files and the date of any change. - - b) You must cause any work that you distribute or publish, that in - whole or in part contains or is derived from the Program or any - part thereof, to be licensed as a whole at no charge to all third - parties under the terms of this License. - - c) If the modified program normally reads commands interactively - when run, you must cause it, when started running for such - interactive use in the most ordinary way, to print or display an - announcement including an appropriate copyright notice and a - notice that there is no warranty (or else, saying that you provide - a warranty) and that users may redistribute the program under - these conditions, and telling the user how to view a copy of this - License. (Exception: if the Program itself is interactive but - does not normally print such an announcement, your work based on - the Program is not required to print an announcement.) - -These requirements apply to the modified work as a whole. If -identifiable sections of that work are not derived from the Program, -and can be reasonably considered independent and separate works in -themselves, then this License, and its terms, do not apply to those -sections when you distribute them as separate works. But when you -distribute the same sections as part of a whole which is a work based -on the Program, the distribution of the whole must be on the terms of -this License, whose permissions for other licensees extend to the -entire whole, and thus to each and every part regardless of who wrote it. - -Thus, it is not the intent of this section to claim rights or contest -your rights to work written entirely by you; rather, the intent is to -exercise the right to control the distribution of derivative or -collective works based on the Program. - -In addition, mere aggregation of another work not based on the Program -with the Program (or with a work based on the Program) on a volume of -a storage or distribution medium does not bring the other work under -the scope of this License. - - 3. You may copy and distribute the Program (or a work based on it, -under Section 2) in object code or executable form under the terms of -Sections 1 and 2 above provided that you also do one of the following: - - a) Accompany it with the complete corresponding machine-readable - source code, which must be distributed under the terms of Sections - 1 and 2 above on a medium customarily used for software interchange; or, - - b) Accompany it with a written offer, valid for at least three - years, to give any third party, for a charge no more than your - cost of physically performing source distribution, a complete - machine-readable copy of the corresponding source code, to be - distributed under the terms of Sections 1 and 2 above on a medium - customarily used for software interchange; or, - - c) Accompany it with the information you received as to the offer - to distribute corresponding source code. (This alternative is - allowed only for noncommercial distribution and only if you - received the program in object code or executable form with such - an offer, in accord with Subsection b above.) - -The source code for a work means the preferred form of the work for -making modifications to it. For an executable work, complete source -code means all the source code for all modules it contains, plus any -associated interface definition files, plus the scripts used to -control compilation and installation of the executable. However, as a -special exception, the source code distributed need not include -anything that is normally distributed (in either source or binary -form) with the major components (compiler, kernel, and so on) of the -operating system on which the executable runs, unless that component -itself accompanies the executable. - -If distribution of executable or object code is made by offering -access to copy from a designated place, then offering equivalent -access to copy the source code from the same place counts as -distribution of the source code, even though third parties are not -compelled to copy the source along with the object code. - - 4. You may not copy, modify, sublicense, or distribute the Program -except as expressly provided under this License. Any attempt -otherwise to copy, modify, sublicense or distribute the Program is -void, and will automatically terminate your rights under this License. -However, parties who have received copies, or rights, from you under -this License will not have their licenses terminated so long as such -parties remain in full compliance. - - 5. You are not required to accept this License, since you have not -signed it. However, nothing else grants you permission to modify or -distribute the Program or its derivative works. These actions are -prohibited by law if you do not accept this License. Therefore, by -modifying or distributing the Program (or any work based on the -Program), you indicate your acceptance of this License to do so, and -all its terms and conditions for copying, distributing or modifying -the Program or works based on it. - - 6. Each time you redistribute the Program (or any work based on the -Program), the recipient automatically receives a license from the -original licensor to copy, distribute or modify the Program subject to -these terms and conditions. You may not impose any further -restrictions on the recipients' exercise of the rights granted herein. -You are not responsible for enforcing compliance by third parties to -this License. - - 7. If, as a consequence of a court judgment or allegation of patent -infringement or for any other reason (not limited to patent issues), -conditions are imposed on you (whether by court order, agreement or -otherwise) that contradict the conditions of this License, they do not -excuse you from the conditions of this License. If you cannot -distribute so as to satisfy simultaneously your obligations under this -License and any other pertinent obligations, then as a consequence you -may not distribute the Program at all. For example, if a patent -license would not permit royalty-free redistribution of the Program by -all those who receive copies directly or indirectly through you, then -the only way you could satisfy both it and this License would be to -refrain entirely from distribution of the Program. - -If any portion of this section is held invalid or unenforceable under -any particular circumstance, the balance of the section is intended to -apply and the section as a whole is intended to apply in other -circumstances. - -It is not the purpose of this section to induce you to infringe any -patents or other property right claims or to contest validity of any -such claims; this section has the sole purpose of protecting the -integrity of the free software distribution system, which is -implemented by public license practices. Many people have made -generous contributions to the wide range of software distributed -through that system in reliance on consistent application of that -system; it is up to the author/donor to decide if he or she is willing -to distribute software through any other system and a licensee cannot -impose that choice. - -This section is intended to make thoroughly clear what is believed to -be a consequence of the rest of this License. - - 8. If the distribution and/or use of the Program is restricted in -certain countries either by patents or by copyrighted interfaces, the -original copyright holder who places the Program under this License -may add an explicit geographical distribution limitation excluding -those countries, so that distribution is permitted only in or among -countries not thus excluded. In such case, this License incorporates -the limitation as if written in the body of this License. - - 9. The Free Software Foundation may publish revised and/or new versions -of the General Public License from time to time. Such new versions will -be similar in spirit to the present version, but may differ in detail to -address new problems or concerns. - -Each version is given a distinguishing version number. If the Program -specifies a version number of this License which applies to it and "any -later version", you have the option of following the terms and conditions -either of that version or of any later version published by the Free -Software Foundation. If the Program does not specify a version number of -this License, you may choose any version ever published by the Free Software -Foundation. - - 10. If you wish to incorporate parts of the Program into other free -programs whose distribution conditions are different, write to the author -to ask for permission. For software which is copyrighted by the Free -Software Foundation, write to the Free Software Foundation; we sometimes -make exceptions for this. Our decision will be guided by the two goals -of preserving the free status of all derivatives of our free software and -of promoting the sharing and reuse of software generally. - - NO WARRANTY - - 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY -FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN -OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES -PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED -OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF -MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS -TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE -PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, -REPAIR OR CORRECTION. - - 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING -WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR -REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, -INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING -OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED -TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY -YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER -PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE -POSSIBILITY OF SUCH DAMAGES. - - END OF TERMS AND CONDITIONS - - How to Apply These Terms to Your New Programs - - If you develop a new program, and you want it to be of the greatest -possible use to the public, the best way to achieve this is to make it -free software which everyone can redistribute and change under these terms. - - To do so, attach the following notices to the program. It is safest -to attach them to the start of each source file to most effectively -convey the exclusion of warranty; and each file should have at least -the "copyright" line and a pointer to where the full notice is found. - - <one line to give the program's name and a brief idea of what it does.> - Copyright (C) 19yy <name of author> - - This program is free software; you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation; either version 2 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program; if not, write to the Free Software - Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - - -Also add information on how to contact you by electronic and paper mail. - -If the program is interactive, make it output a short notice like this -when it starts in an interactive mode: - - Gnomovision version 69, Copyright (C) 19yy name of author - Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. - This is free software, and you are welcome to redistribute it - under certain conditions; type `show c' for details. - -The hypothetical commands `show w' and `show c' should show the appropriate -parts of the General Public License. Of course, the commands you use may -be called something other than `show w' and `show c'; they could even be -mouse-clicks or menu items--whatever suits your program. - -You should also get your employer (if you work as a programmer) or your -school, if any, to sign a "copyright disclaimer" for the program, if -necessary. Here is a sample; alter the names: - - Yoyodyne, Inc., hereby disclaims all copyright interest in the program - `Gnomovision' (which makes passes at compilers) written by James Hacker. - - <signature of Ty Coon>, 1 April 1989 - Ty Coon, President of Vice - -This General Public License does not permit incorporating your program into -proprietary programs. If your program is a subroutine library, you may -consider it more useful to permit linking proprietary applications with the -library. If this is what you want to do, use the GNU Library General -Public License instead of this License. Modified: code/player/trunk/server/drivers/camera/camfilter/camfilter.cc =================================================================== --- code/player/trunk/server/drivers/camera/camfilter/camfilter.cc 2009-06-08 06:20:28 UTC (rev 7814) +++ code/player/trunk/server/drivers/camera/camfilter/camfilter.cc 2009-06-08 06:31:35 UTC (rev 7815) @@ -148,7 +148,8 @@ #include <assert.h> #include <pthread.h> #include <libplayercore/playercore.h> -#if HAVE_JPEGLIB_H +#include <config.h> +#if HAVE_JPEG #include <libplayerjpeg/playerjpeg.h> #else #warning JPEG decompression disabled! @@ -396,7 +397,7 @@ return -1; } break; -#if HAVE_JPEGLIB_H +#if HAVE_JPEG case PLAYER_CAMERA_COMPRESS_JPEG: if (this->bufsize != new_size) { @@ -509,16 +510,3 @@ } return -1; } - -#ifdef JUST_A_PLUGIN - -extern "C" -{ - int player_driver_init(DriverTable * table) - { - CamFilter_Register(table); - return 0; - } -} - -#endif Deleted: code/player/trunk/server/drivers/camera/camfilter/version.txt =================================================================== --- code/player/trunk/server/drivers/camera/camfilter/version.txt 2009-06-08 06:20:28 UTC (rev 7814) +++ code/player/trunk/server/drivers/camera/camfilter/version.txt 2009-06-08 06:31:35 UTC (rev 7815) @@ -1 +0,0 @@ -20090315 Deleted: code/player/trunk/server/drivers/dio/serio/AUTHORS =================================================================== --- code/player/trunk/server/drivers/dio/serio/AUTHORS 2009-06-08 06:20:28 UTC (rev 7814) +++ code/player/trunk/server/drivers/dio/serio/AUTHORS 2009-06-08 06:31:35 UTC (rev 7815) @@ -1 +0,0 @@ -Paul Osmialowski <new...@ki...> Deleted: code/player/trunk/server/drivers/dio/serio/COPYING =================================================================== --- code/player/trunk/server/drivers/dio/serio/COPYING 2009-06-08 06:20:28 UTC (rev 7814) +++ code/player/trunk/server/drivers/dio/serio/COPYING 2009-06-08 06:31:35 UTC (rev 7815) @@ -1,340 +0,0 @@ - GNU GENERAL PUBLIC LICENSE - Version 2, June 1991 - - Copyright (C) 1989, 1991 Free Software Foundation, Inc. - 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - Everyone is permitted to copy and distribute verbatim copies - of this license document, but changing it is not allowed. - - Preamble - - The licenses for most software are designed to take away your -freedom to share and change it. By contrast, the GNU General Public -License is intended to guarantee your freedom to share and change free -software--to make sure the software is free for all its users. This -General Public License applies to most of the Free Software -Foundation's software and to any other program whose authors commit to -using it. (Some other Free Software Foundation software is covered by -the GNU Library General Public License instead.) You can apply it to -your programs, too. - - When we speak of free software, we are referring to freedom, not -price. Our General Public Licenses are designed to make sure that you -have the freedom to distribute copies of free software (and charge for -this service if you wish), that you receive source code or can get it -if you want it, that you can change the software or use pieces of it -in new free programs; and that you know you can do these things. - - To protect your rights, we need to make restrictions that forbid -anyone to deny you these rights or to ask you to surrender the rights. -These restrictions translate to certain responsibilities for you if you -distribute copies of the software, or if you modify it. - - For example, if you distribute copies of such a program, whether -gratis or for a fee, you must give the recipients all the rights that -you have. You must make sure that they, too, receive or can get the -source code. And you must show them these terms so they know their -rights. - - We protect your rights with two steps: (1) copyright the software, and -(2) offer you this license which gives you legal permission to copy, -distribute and/or modify the software. - - Also, for each author's protection and ours, we want to make certain -that everyone understands that there is no warranty for this free -software. If the software is modified by someone else and passed on, we -want its recipients to know that what they have is not the original, so -that any problems introduced by others will not reflect on the original -authors' reputations. - - Finally, any free program is threatened constantly by software -patents. We wish to avoid the danger that redistributors of a free -program will individually obtain patent licenses, in effect making the -program proprietary. To prevent this, we have made it clear that any -patent must be licensed for everyone's free use or not licensed at all. - - The precise terms and conditions for copying, distribution and -modification follow. - - GNU GENERAL PUBLIC LICENSE - TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION - - 0. This License applies to any program or other work which contains -a notice placed by the copyright holder saying it may be distributed -under the terms of this General Public License. The "Program", below, -refers to any such program or work, and a "work based on the Program" -means either the Program or any derivative work under copyright law: -that is to say, a work containing the Program or a portion of it, -either verbatim or with modifications and/or translated into another -language. (Hereinafter, translation is included without limitation in -the term "modification".) Each licensee is addressed as "you". - -Activities other than copying, distribution and modification are not -covered by this License; they are outside its scope. The act of -running the Program is not restricted, and the output from the Program -is covered only if its contents constitute a work based on the -Program (independent of having been made by running the Program). -Whether that is true depends on what the Program does. - - 1. You may copy and distribute verbatim copies of the Program's -source code as you receive it, in any medium, provided that you -conspicuously and appropriately publish on each copy an appropriate -copyright notice and disclaimer of warranty; keep intact all the -notices that refer to this License and to the absence of any warranty; -and give any other recipients of the Program a copy of this License -along with the Program. - -You may charge a fee for the physical act of transferring a copy, and -you may at your option offer warranty protection in exchange for a fee. - - 2. You may modify your copy or copies of the Program or any portion -of it, thus forming a work based on the Program, and copy and -distribute such modifications or work under the terms of Section 1 -above, provided that you also meet all of these conditions: - - a) You must cause the modified files to carry prominent notices - stating that you changed the files and the date of any change. - - b) You must cause any work that you distribute or publish, that in - whole or in part contains or is derived from the Program or any - part thereof, to be licensed as a whole at no charge to all third - parties under the terms of this License. - - c) If the modified program normally reads commands interactively - when run, you must cause it, when started running for such - interactive use in the most ordinary way, to print or display an - announcement including an appropriate copyright notice and a - notice that there is no warranty (or else, saying that you provide - a warranty) and that users may redistribute the program under - these conditions, and telling the user how to view a copy of this - License. (Exception: if the Program itself is interactive but - does not normally print such an announcement, your work based on - the Program is not required to print an announcement.) - -These requirements apply to the modified work as a whole. If -identifiable sections of that work are not derived from the Program, -and can be reasonably considered independent and separate works in -themselves, then this License, and its terms, do not apply to those -sections when you distribute them as separate works. But when you -distribute the same sections as part of a whole which is a work based -on the Program, the distribution of the whole must be on the terms of -this License, whose permissions for other licensees extend to the -entire whole, and thus to each and every part regardless of who wrote it. - -Thus, it is not the intent of this section to claim rights or contest -your rights to work written entirely by you; rather, the intent is to -exercise the right to control the distribution of derivative or -collective works based on the Program. - -In addition, mere aggregation of another work not based on the Program -with the Program (or with a work based on the Program) on a volume of -a storage or distribution medium does not bring the other work under -the scope of this License. - - 3. You may copy and distribute the Program (or a work based on it, -under Section 2) in object code or executable form under the terms of -Sections 1 and 2 above provided that you also do one of the following: - - a) Accompany it with the complete corresponding machine-readable - source code, which must be distributed under the terms of Sections - 1 and 2 above on a medium customarily used for software interchange; or, - - b) Accompany it with a written offer, valid for at least three - years, to give any third party, for a charge no more than your - cost of physically performing source distribution, a complete - machine-readable copy of the corresponding source code, to be - distributed under the terms of Sections 1 and 2 above on a medium - customarily used for software interchange; or, - - c) Accompany it with the information you received as to the offer - to distribute corresponding source code. (This alternative is - allowed only for noncommercial distribution and only if you - received the program in object code or executable form with such - an offer, in accord with Subsection b above.) - -The source code for a work means the preferred form of the work for -making modifications to it. For an executable work, complete source -code means all the source code for all modules it contains, plus any -associated interface definition files, plus the scripts used to -control compilation and installation of the executable. However, as a -special exception, the source code distributed need not include -anything that is normally distributed (in either source or binary -form) with the major components (compiler, kernel, and so on) of the -operating system on which the executable runs, unless that component -itself accompanies the executable. - -If distribution of executable or object code is made by offering -access to copy from a designated place, then offering equivalent -access to copy the source code from the same place counts as -distribution of the source code, even though third parties are not -compelled to copy the source along with the object code. - - 4. You may not copy, modify, sublicense, or distribute the Program -except as expressly provided under this License. Any attempt -otherwise to copy, modify, sublicense or distribute the Program is -void, and will automatically terminate your rights under this License. -However, parties who have received copies, or rights, from you under -this License will not have their licenses terminated so long as such -parties remain in full compliance. - - 5. You are not required to accept this License, since you have not -signed it. However, nothing else grants you permission to modify or -distribute the Program or its derivative works. These actions are -prohibited by law if you do not accept this License. Therefore, by -modifying or distributing the Program (or any work based on the -Program), you indicate your acceptance of this License to do so, and -all its terms and conditions for copying, distributing or modifying -the Program or works based on it. - - 6. Each time you redistribute the Program (or any work based on the -Program), the recipient automatically receives a license from the -original licensor to copy, distribute or modify the Program subject to -these terms and conditions. You may not impose any further -restrictions on the recipients' exercise of the rights granted herein. -You are not responsible for enforcing compliance by third parties to -this License. - - 7. If, as a consequence of a court judgment or allegation of patent -infringement or for any other reason (not limited to patent issues), -conditions are imposed on you (whether by court order, agreement or -otherwise) that contradict the conditions of this License, they do not -excuse you from the conditions of this License. If you cannot -distribute so as to satisfy simultaneously your obligations under this -License and any other pertinent obligations, then as a consequence you -may not distribute the Program at all. For example, if a patent -license would not permit royalty-free redistribution of the Program by -all those who receive copies directly or indirectly through you, then -the only way you could satisfy both it and this License would be to -refrain entirely from distribution of the Program. - -If any portion of this section is held invalid or unenforceable under -any particular circumstance, the balance of the section is intended to -apply and the section as a whole is intended to apply in other -circumstances. - -It is not the purpose of this section to induce you to infringe any -patents or other property right claims or to contest validity of any -such claims; this section has the sole purpose of protecting the -integrity of the free software distribution system, which is -implemented by public license practices. Many people have made -generous contributions to the wide range of software distributed -through that system in reliance on consistent application of that -system; it is up to the author/donor to decide if he or she is willing -to distribute software through any other system and a licensee cannot -impose that choice. - -This section is intended to make thoroughly clear what is believed to -be a consequence of the rest of this License. - - 8. If the distribution and/or use of the Program is restricted in -certain countries either by patents or by copyrighted interfaces, the -original copyright holder who places the Program under this License -may add an explicit geographical distribution limitation excluding -those countries, so that distribution is permitted only in or among -countries not thus excluded. In such case, this License incorporates -the limitation as if written in the body of this License. - - 9. The Free Software Foundation may publish revised and/or new versions -of the General Public License from time to time. Such new versions will -be similar in spirit to the present version, but may differ in detail to -address new problems or concerns. - -Each version is given a distinguishing version number. If the Program -specifies a version number of this License which applies to it and "any -later version", you have the option of following the terms and conditions -either of that version or of any later version published by the Free -Software Foundation. If the Program does not specify a version number of -this License, you may choose any version ever published by the Free Software -Foundation. - - 10. If you wish to incorporate parts of the Program into other free -programs whose distribution conditions are different, write to the author -to ask for permission. For software which is copyrighted by the Free -Software Foundation, write to the Free Software Foundation; we sometimes -make exceptions for this. Our decision will be guided by the two goals -of preserving the free status of all derivatives of our free software and -of promoting the sharing and reuse of software generally. - - NO WARRANTY - - 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY -FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN -OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES -PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED -OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF -MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS -TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE -PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, -REPAIR OR CORRECTION. - - 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING -WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR -REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, -INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING -OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED -TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY -YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER -PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE -POSSIBILITY OF SUCH DAMAGES. - - END OF TERMS AND CONDITIONS - - How to Apply These Terms to Your New Programs - - If you develop a new program, and you want it to be of the greatest -possible use to the public, the best way to achieve this is to make it -free software which everyone can redistribute and change under these terms. - - To do so, attach the following notices to the program. It is safest -to attach them to the start of each source file to most effectively -convey the exclusion of warranty; and each file should have at least -the "copyright" line and a pointer to where the full notice is found. - - <one line to give the program's name and a brief idea of what it does.> - Copyright (C) 19yy <name of author> - - This program is free software; you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation; either version 2 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program; if not, write to the Free Software - Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - - -Also add information on how to contact you by electronic and paper mail. - -If the program is interactive, make it output a short notice like this -when it starts in an interactive mode: - - Gnomovision version 69, Copyright (C) 19yy name of author - Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. - This is free software, and you are welcome to redistribute it - under certain conditions; type `show c' for details. - -The hypothetical commands `show w' and `show c' should show the appropriate -parts of the General Public License. Of course, the commands you use may -be called something other than `show w' and `show c'; they could even be -mouse-clicks or menu items--whatever suits your program. - -You should also get your employer (if you work as a programmer) or your -school, if any, to sign a "copyright disclaimer" for the program, if -necessary. Here is a sample; alter the names: - - Yoyodyne, Inc., hereby disclaims all copyright interest in the program - `Gnomovision' (which makes passes at compilers) written by James Hacker. - - <signature of Ty Coon>, 1 April 1989 - Ty Coon, President of Vice - -This General Public License does not permit incorporating your program into -proprietary programs. If your program is a subroutine library, you may -consider it more useful to permit linking proprietary applications with the -library. If this is what you want to do, use the GNU Library General -Public License instead of this License. Deleted: code/player/trunk/server/drivers/dio/serio/version.txt =================================================================== --- code/player/trunk/server/drivers/dio/serio/version.txt 2009-06-08 06:20:28 UTC (rev 7814) +++ code/player/trunk/server/drivers/dio/serio/version.txt 2009-06-08 06:31:35 UTC (rev 7815) @@ -1 +0,0 @@ -20090403 This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <gb...@us...> - 2009-06-10 00:06:29
|
Revision: 7829 http://playerstage.svn.sourceforge.net/playerstage/?rev=7829&view=rev Author: gbiggs Date: 2009-06-10 00:06:24 +0000 (Wed, 10 Jun 2009) Log Message: ----------- Applied patch #2803697 Modified Paths: -------------- code/player/trunk/server/drivers/mixed/epuck/epuckInterface.cpp code/player/trunk/server/drivers/mixed/epuck/epuckInterface.hpp code/player/trunk/server/drivers/mixed/epuck/epuckPosition2d.cpp code/player/trunk/server/drivers/mixed/epuck/epuckPosition2d.hpp code/player/trunk/server/drivers/position/snd/gap_and_valley.h Modified: code/player/trunk/server/drivers/mixed/epuck/epuckInterface.cpp =================================================================== --- code/player/trunk/server/drivers/mixed/epuck/epuckInterface.cpp 2009-06-09 23:57:21 UTC (rev 7828) +++ code/player/trunk/server/drivers/mixed/epuck/epuckInterface.cpp 2009-06-10 00:06:24 UTC (rev 7829) @@ -15,6 +15,8 @@ #include "epuckInterface.hpp" +const float EpuckInterface::EPUCK_DIAMETER = 0.07; + EpuckInterface::EpuckInterface(const SerialPort* const serialPort) :serialPort(serialPort) {} Modified: code/player/trunk/server/drivers/mixed/epuck/epuckInterface.hpp =================================================================== --- code/player/trunk/server/drivers/mixed/epuck/epuckInterface.hpp 2009-06-09 23:57:21 UTC (rev 7828) +++ code/player/trunk/server/drivers/mixed/epuck/epuckInterface.hpp 2009-06-10 00:06:24 UTC (rev 7829) @@ -59,7 +59,7 @@ const SerialPort* const serialPort; /// Diameter of e-puck body [m]. - static const float EPUCK_DIAMETER = 0.07; + static const float EPUCK_DIAMETER; /// Request codes acceptable by e-puck. enum Request Modified: code/player/trunk/server/drivers/mixed/epuck/epuckPosition2d.cpp =================================================================== --- code/player/trunk/server/drivers/mixed/epuck/epuckPosition2d.cpp 2009-06-09 23:57:21 UTC (rev 7828) +++ code/player/trunk/server/drivers/mixed/epuck/epuckPosition2d.cpp 2009-06-10 00:06:24 UTC (rev 7829) @@ -20,6 +20,17 @@ #define PI 3.141592654 #define DOIS_PI 6.283185308 +// Diameter of e-puck wheels [m] +const float EpuckPosition2d::WHEEL_DIAMETER = 0.0412; +// Distance between e-puck wheels [m] +const float EpuckPosition2d::TRACK = 0.05255; +// Wheel radius divided by TRACK [m] +const float EpuckPosition2d::r_DIV_L = 0.392007612; +// Half of wheel radius [m] +const float EpuckPosition2d::r_DIV_2 = 0.0103; +// Angular displacement of one motor step [rad] +const float EpuckPosition2d::STEP_ANG_DISP = 6.283185308e-3; + EpuckPosition2d::EpuckPosition2d(const SerialPort* const serialPort) :EpuckInterface(serialPort) { Modified: code/player/trunk/server/drivers/mixed/epuck/epuckPosition2d.hpp =================================================================== --- code/player/trunk/server/drivers/mixed/epuck/epuckPosition2d.hpp 2009-06-09 23:57:21 UTC (rev 7828) +++ code/player/trunk/server/drivers/mixed/epuck/epuckPosition2d.hpp 2009-06-10 00:06:24 UTC (rev 7829) @@ -112,19 +112,19 @@ BodyGeometry geometry; // Diameter of e-puck wheels [m] - static const float WHEEL_DIAMETER = 0.0412; + static const float WHEEL_DIAMETER; // Distance between e-puck wheels [m] - static const float TRACK = 0.05255; + static const float TRACK; // Wheel radius divided by TRACK [m] - static const float r_DIV_L = 0.392007612; + static const float r_DIV_L; // Half of wheel radius [m] - static const float r_DIV_2 = 0.0103; + static const float r_DIV_2; // Angular displacement of one motor step [rad] - static const float STEP_ANG_DISP = 6.283185308e-3; + static const float STEP_ANG_DISP; }; #endif Modified: code/player/trunk/server/drivers/position/snd/gap_and_valley.h =================================================================== --- code/player/trunk/server/drivers/position/snd/gap_and_valley.h 2009-06-09 23:57:21 UTC (rev 7828) +++ code/player/trunk/server/drivers/position/snd/gap_and_valley.h 2009-06-10 00:06:24 UTC (rev 7829) @@ -30,10 +30,8 @@ extern int getSectorsBetween( int iS1, int iS2, int iSMax ); extern int getSectorsBetweenDirected( int iS1, int iS2, int iSMax, int iDirection ); -class Gap{ - protected : - ; - +class Gap +{ public : int m_iSector; double m_dist; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2009-06-10 15:09:52
|
Revision: 7833 http://playerstage.svn.sourceforge.net/playerstage/?rev=7833&view=rev Author: thjc Date: 2009-06-10 15:09:50 +0000 (Wed, 10 Jun 2009) Log Message: ----------- disable snd and epuck if stl not present Modified Paths: -------------- code/player/trunk/server/drivers/mixed/epuck/CMakeLists.txt code/player/trunk/server/drivers/position/snd/CMakeLists.txt Modified: code/player/trunk/server/drivers/mixed/epuck/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/mixed/epuck/CMakeLists.txt 2009-06-10 14:15:38 UTC (rev 7832) +++ code/player/trunk/server/drivers/mixed/epuck/CMakeLists.txt 2009-06-10 15:09:50 UTC (rev 7833) @@ -1,3 +1,8 @@ -PLAYERDRIVER_OPTION (epuck build_epuck ON) +IF (HAVE_STL) + PLAYERDRIVER_OPTION (epuck build_epuck ON) +ELSE (HAVE_STL) + PLAYERDRIVER_OPTION (epuck build_epuck OFF "STL not found") +ENDIF (HAVE_STL) + PLAYERDRIVER_REJECT_OS (epuck build_epuck PLAYER_OS_WIN) PLAYERDRIVER_ADD_DRIVER (epuck build_epuck SOURCES epuckCamera.cpp epuckDriver.cpp epuckInterface.cpp epuckIR.cpp epuckLEDs.cpp epuckPosition2d.cpp serialPort.cpp) Modified: code/player/trunk/server/drivers/position/snd/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/position/snd/CMakeLists.txt 2009-06-10 14:15:38 UTC (rev 7832) +++ code/player/trunk/server/drivers/position/snd/CMakeLists.txt 2009-06-10 15:09:50 UTC (rev 7833) @@ -1,2 +1,6 @@ -PLAYERDRIVER_OPTION (snd build_snd ON) +IF (HAVE_STL) + PLAYERDRIVER_OPTION (snd build_snd ON) +ELSE (HAVE_STL) + PLAYERDRIVER_OPTION (snd build_snd OFF "STL not found") +ENDIF (HAVE_STL) PLAYERDRIVER_ADD_DRIVER (snd build_snd SOURCES gap_and_valley.cc gap_nd_nav.cc snd.cc ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <gb...@us...> - 2009-06-24 14:10:02
|
Revision: 7888 http://playerstage.svn.sourceforge.net/playerstage/?rev=7888&view=rev Author: gbiggs Date: 2009-06-24 13:25:20 +0000 (Wed, 24 Jun 2009) Log Message: ----------- Added drivers for GbxGarminAcfr and GbxSmartBatteryAcfr libs Modified Paths: -------------- code/player/trunk/server/drivers/CMakeLists.txt code/player/trunk/server/drivers/gps/CMakeLists.txt code/player/trunk/server/drivers/ranger/gbxsickacfr.cc Added Paths: ----------- code/player/trunk/server/drivers/gps/gbxgarminacfr.cc code/player/trunk/server/drivers/power/ code/player/trunk/server/drivers/power/CMakeLists.txt code/player/trunk/server/drivers/power/oceanserver.cc Modified: code/player/trunk/server/drivers/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/CMakeLists.txt 2009-06-24 13:21:49 UTC (rev 7887) +++ code/player/trunk/server/drivers/CMakeLists.txt 2009-06-24 13:25:20 UTC (rev 7888) @@ -23,6 +23,7 @@ ADD_SUBDIRECTORY (planner) ADD_SUBDIRECTORY (pointcloud3d) ADD_SUBDIRECTORY (position) +ADD_SUBDIRECTORY (power) ADD_SUBDIRECTORY (ptz) ADD_SUBDIRECTORY (ranger) ADD_SUBDIRECTORY (rfid) Modified: code/player/trunk/server/drivers/gps/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/gps/CMakeLists.txt 2009-06-24 13:21:49 UTC (rev 7887) +++ code/player/trunk/server/drivers/gps/CMakeLists.txt 2009-06-24 13:25:20 UTC (rev 7888) @@ -1,3 +1,13 @@ PLAYERDRIVER_OPTION (garminnmea build_garminnmea ON) PLAYERDRIVER_REJECT_OS (garminnmea build_garminnmea PLAYER_OS_WIN) PLAYERDRIVER_ADD_DRIVER (garminnmea build_garminnmea SOURCES garminnmea.cc) + +PLAYERDRIVER_OPTION (gbxgarminacfr build_gbxgarminacfr ON) +PLAYERDRIVER_REQUIRE_PKG (gbxgarminacfr build_gbxgarminacfr GbxGarminAcfr + gbxgarminacfr_includeDirs gbxgarminacfr_libDirs gbxgarminacfr_linkLibs + gbxgarminacfr_linkFlags gbxgarminacfr_cFlags) +PLAYERDRIVER_ADD_DRIVER (gbxgarminacfr build_gbxgarminacfr + INCLUDEDIRS "${gbxgarminacfr_includeDirs}" LIBDIRS "${gbxgarminacfr_libDirs}" + LINKLIBS "${gbxgarminacfr_linkLibs}" LINKFLAGS "${gbxgarminacfr_linkFlags}" + CFLAGS "${gbxgarminacfr_cFlags}" SOURCES gbxgarminacfr.cc) + Added: code/player/trunk/server/drivers/gps/gbxgarminacfr.cc =================================================================== --- code/player/trunk/server/drivers/gps/gbxgarminacfr.cc (rev 0) +++ code/player/trunk/server/drivers/gps/gbxgarminacfr.cc 2009-06-24 13:25:20 UTC (rev 7888) @@ -0,0 +1,288 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000-2003 Brian Gerkey ge...@us... + * Andrew Howard ah...@us... + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +/////////////////////////////////////////////////////////////////////////// +// +// Desc: Driver wrapper around the Gearbox gbxgarminacfr library. +// Author: Geoffrey Biggs +// Date: 24/06/2009 +// +// Provides - GPS device. +// +/////////////////////////////////////////////////////////////////////////// + +/** @ingroup drivers */ +/** @{ */ +/** @defgroup driver_gbxgarminacfr gbxgarminacfr + * @brief Gearbox Garmin GPS driver + +This driver provides a @ref interface_gps interface to Garmin GPS devices, +as well as any other GPS device supported by the GbxGarminAcfr library. This +should include most GPS devices that use NMEA to communicate. + +@par Compile-time dependencies + +- Gearbox library GbxGarminAcfr + +@par Provides + +- @ref interface_gps: Output GPS interface + +@par Supported configuration requests + +- None. + +@par Configuration file options + + - read_gga (boolean) + - Default: true + - Read and parse GGA messages. + - read_vtg (boolean) + - Default: true + - Read and parse VTG messages. + - read_rme (boolean) + - Default: true + - Read and parse RME messages. + - ignore_unknown (boolean) + - Default: false + - Silently ignore unknown messages. + - port (string) + - Default: /dev/ttyS0 + - Serial port the laser is connected to. + - debug (int) + - Default: 0 + - Debugging level of the underlying library to get verbose output. + - pose (float 6-tuple: (m, m, m, rad, rad, rad)) + - Default: [0.0 0.0 0.0 0.0 0.0 0.0] + - Pose (x, y, z, roll, pitch, yaw) of the laser relative to its parent object (e.g. the robot). + - size (float 3-tuple: (m, m, m)) + - Default: [0.0 0.0 0.0] + - Size of the laser in metres. + +@par Example + +@verbatim +driver +( + name "gbxgarminacfr" + provides ["gps:0"] + port "/dev/ttyS0" +) +@endverbatim + +@author Geoffrey Biggs + +*/ +/** @} */ + +#include <gbxgarminacfr/driver.h> +#include <gbxutilacfr/trivialtracer.h> +#include <gbxutilacfr/trivialstatus.h> +#include <gbxutilacfr/mathdefs.h> + +#include <libplayercore/playercore.h> + +class GbxGarminAcfr : public ThreadedDriver +{ + public: + GbxGarminAcfr (ConfigFile* cf, int section); + ~GbxGarminAcfr (void); + + virtual int MainSetup (void); + virtual void MainQuit (void); + virtual int ProcessMessage (QueuePointer &resp_queue, player_msghdr *hdr, void *data); + + private: + virtual void Main (void); + bool ReadSensor (void); + + // Configuration parameters + gbxgarminacfr::Config _config; + unsigned int _debug; + // Geometry + player_ranger_geom_t _geom; + player_pose3d_t _sensorPose; + player_bbox3d_t _sensorSize; + // Data storage + player_gps_data_t _gpsData; + // The hardware device itself + std::auto_ptr<gbxgarminacfr::Driver> _device; + // Objects to handle messages from the driver + std::auto_ptr<gbxutilacfr::TrivialTracer> _tracer; + std::auto_ptr<gbxutilacfr::TrivialStatus> _status; +}; + +Driver* +GbxGarminAcfr_Init (ConfigFile* cf, int section) +{ + return reinterpret_cast <Driver*> (new GbxGarminAcfr (cf, section)); +} + +void gbxgarminacfr_Register(DriverTable* table) +{ + table->AddDriver ("gbxgarminacfr", GbxGarminAcfr_Init); +} + +GbxGarminAcfr::GbxGarminAcfr (ConfigFile* cf, int section) + : ThreadedDriver (cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_RANGER_CODE) +{ + // Setup config object + _config.readGga = cf->ReadFloat (section, "read_gga", true); + _config.readVtg = cf->ReadFloat (section, "read_vtg", true); + _config.readRme = cf->ReadFloat (section, "read_rme", true); + _config.ignoreUnknown = cf->ReadInt (section, "ignore_unknown", false); + _config.device = cf->ReadString (section, "port", "/dev/ttyS0"); + _debug = cf->ReadBool (section, "debug", 0); + // Set up geometry information + _geom.pose.px = cf->ReadTupleLength (section, "pose", 0, 0.0f); + _geom.pose.py = cf->ReadTupleLength (section, "pose", 1, 0.0f); + _geom.pose.pz = cf->ReadTupleLength (section, "pose", 2, 0.0f); + _geom.pose.proll = cf->ReadTupleAngle (section, "pose", 3, 0.0f); + _geom.pose.ppitch = cf->ReadTupleAngle (section, "pose", 4, 0.0f); + _geom.pose.pyaw = cf->ReadTupleAngle (section, "pose", 5, 0.0f); + _geom.size.sw = cf->ReadTupleLength (section, "size", 0, 0.0f); + _geom.size.sl = cf->ReadTupleLength (section, "size", 1, 0.0f); + _geom.size.sh = cf->ReadTupleLength (section, "size", 2, 0.0f); + _geom.sensor_poses_count = 1; + _geom.sensor_poses = &_sensorPose; + memcpy (_geom.sensor_poses, &_geom.pose, sizeof (_geom.pose)); + _geom.sensor_sizes_count = 1; + _geom.sensor_sizes = &_sensorSize; + memcpy (_geom.sensor_sizes, &_geom.size, sizeof (_geom.size)); + + memset (&_gpsData, 0, sizeof (_gpsData)); +} + +GbxGarminAcfr::~GbxGarminAcfr (void) +{ +} + +int GbxGarminAcfr::MainSetup (void) +{ + // Validate the configuration + if (!_config.isValid ()) + { + PLAYER_ERROR ("GbxGarminAcfr: Invalid sensor configuration.\n"); + return -1; + } + + // Create status trackers + _tracer.reset (new gbxutilacfr::TrivialTracer (_debug)); + _status.reset (new gbxutilacfr::TrivialStatus (*_tracer)); + + // Create the driver object + try + { + _device.reset (new gbxgarminacfr::Driver (_config, *_tracer, *_status)); + } + catch (const std::exception& e) + { + PLAYER_ERROR1 ("GbxGarminAcfr: Failed to initialise GPS device: %s\n", e.what ()); + return -1; + } + + return 0; +} + +void GbxGarminAcfr::MainQuit (void) +{ + _device.reset (NULL); + _status.reset (NULL); + _tracer.reset (NULL); +} + +int GbxGarminAcfr::ProcessMessage (QueuePointer &resp_queue, player_msghdr *hdr, void *data) +{ + // Check for capability requests + HANDLE_CAPABILITY_REQUEST (device_addr, resp_queue, hdr, data, PLAYER_MSGTYPE_REQ, PLAYER_CAPABILTIES_REQ); + return -1; +} + +void GbxGarminAcfr::Main (void) +{ + while (true) + { + pthread_testcancel (); + ProcessMessages (); + + if (!ReadSensor ()) + break; + } +} + +bool GbxGarminAcfr::ReadSensor (void) +{ + std::auto_ptr<gbxgarminacfr::GenericData> data; + + try + { + data = _device->read (); + + // find out which data type it is + switch (data->type ()) + { + case gbxgarminacfr::GpGga: + { + gbxgarminacfr::GgaData *d = reinterpret_cast<gbxgarminacfr::GgaData*> (data.get ()); + _gpsData.time_sec = d->timeStampSec; + _gpsData.time_usec = d->timeStampUsec; + _gpsData.latitude = static_cast<int32_t> (d->latitude / 1e7); + _gpsData.longitude = static_cast<int32_t> (d->longitude / 1e7); + _gpsData.altitude = static_cast<int32_t> (d->altitude * 1000); + _gpsData.num_sats = d->satellites; + _gpsData.hdop = static_cast<uint32_t> (d->horizontalDilutionOfPosition * 10); + if (d->fixType == gbxgarminacfr::Autonomous) + _gpsData.quality = 1; + else if (d->fixType == gbxgarminacfr::Differential) + _gpsData.quality = 2; + else + _gpsData.quality = 0; + break; + } + case gbxgarminacfr::GpVtg: + { + gbxgarminacfr::VtgData *d = reinterpret_cast<gbxgarminacfr::VtgData*> (data.get ()); + _gpsData.time_sec = d->timeStampSec; + _gpsData.time_usec = d->timeStampUsec; + break; + } + case gbxgarminacfr::PgRme: + { + gbxgarminacfr::RmeData *d = reinterpret_cast<gbxgarminacfr::RmeData*> (data.get ()); + _gpsData.time_sec = d->timeStampSec; + _gpsData.time_usec = d->timeStampUsec; + _gpsData.err_horz = d->horizontalPositionError; + _gpsData.err_vert = d->verticalPositionError; + break; + } + default: + PLAYER_WARN ("GbxGarminAcfr: Unknown message type received from GPS sensor."); + } + Publish (device_addr, PLAYER_MSGTYPE_DATA, PLAYER_GPS_DATA_STATE, reinterpret_cast<void*> (&_gpsData), sizeof (_gpsData), NULL); + } + catch (const std::exception &e) + { + PLAYER_ERROR1 ("GbxGarminAcfr: Failed to read data: %s\n", e.what ()); + return false; + } + + return true; +} Added: code/player/trunk/server/drivers/power/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/power/CMakeLists.txt (rev 0) +++ code/player/trunk/server/drivers/power/CMakeLists.txt 2009-06-24 13:25:20 UTC (rev 7888) @@ -0,0 +1,9 @@ +PLAYERDRIVER_OPTION (oceanserver build_oceanserver ON) +PLAYERDRIVER_REQUIRE_PKG (oceanserver build_oceanserver GbxSmartBatteryAcfr + gbxsmartbatteryacfr_includeDirs gbxsmartbatteryacfr_libDirs gbxsmartbatteryacfr_linkLibs + gbxsmartbatteryacfr_linkFlags gbxsmartbatteryacfr_cFlags) +PLAYERDRIVER_ADD_DRIVER (oceanserver build_oceanserver + INCLUDEDIRS "${gbxsmartbatteryacfr_includeDirs}" LIBDIRS "${gbxsmartbatteryacfr_libDirs}" + LINKLIBS "${gbxsmartbatteryacfr_linkLibs}" LINKFLAGS "${gbxsmartbatteryacfr_linkFlags}" + CFLAGS "${gbxsmartbatteryacfr_cFlags}" SOURCES oceanserver.cc) + Added: code/player/trunk/server/drivers/power/oceanserver.cc =================================================================== --- code/player/trunk/server/drivers/power/oceanserver.cc (rev 0) +++ code/player/trunk/server/drivers/power/oceanserver.cc 2009-06-24 13:25:20 UTC (rev 7888) @@ -0,0 +1,257 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000-2003 Brian Gerkey ge...@us... + * Andrew Howard ah...@us... + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +/////////////////////////////////////////////////////////////////////////// +// +// Desc: Driver wrapper around the Gearbox gbxsmartbatteryacfr library. +// Author: Geoffrey Biggs +// Date: 24/06/2009 +// +// Provides - Power device. +// +/////////////////////////////////////////////////////////////////////////// + +/** @ingroup drivers */ +/** @{ */ +/** @defgroup driver_gbxsmartbatteryacfr gbxsmartbatteryacfr + * @brief Gearbox SmartBattery driver for OceanServer devices. + +This driver provides a @ref interface_power interface to the OceanServer +Smart Battery systems supported by the GbxSmartBatteryAcfr library. + +@par Compile-time dependencies + +- Gearbox library GbxSmartBatteryAcfr + +@par Provides + +- @ref interface_power: Output power interface + +@par Supported configuration requests + +- None. + +@par Configuration file options + + - port (string) + - Default: /dev/ttyS0 + - Serial port the laser is connected to. + - debug (int) + - Default: 0 + - Debugging level of the underlying library to get verbose output. + - pose (float 6-tuple: (m, m, m, rad, rad, rad)) + - Default: [0.0 0.0 0.0 0.0 0.0 0.0] + - Pose (x, y, z, roll, pitch, yaw) of the laser relative to its parent object (e.g. the robot). + - size (float 3-tuple: (m, m, m)) + - Default: [0.0 0.0 0.0] + - Size of the laser in metres. + +@par Example + +@verbatim +driver +( + name "gbxsmartbatteryacfr" + provides ["power:0"] + port "/dev/ttyS0" +) +@endverbatim + +@author Geoffrey Biggs + +*/ +/** @} */ + +#include <string> +#include <gbxutilacfr/trivialtracer.h> +#include <gbxsmartbatteryacfr/gbxsmartbatteryacfr.h> + +#include <libplayercore/playercore.h> + +class OceanServer : public ThreadedDriver +{ + public: + OceanServer (ConfigFile* cf, int section); + ~OceanServer (void); + + virtual int MainSetup (void); + virtual void MainQuit (void); + virtual int ProcessMessage (QueuePointer &resp_queue, player_msghdr *hdr, void *data); + + private: + virtual void Main (void); + bool ReadSensor (void); + + // Configuration parameters + std::string _port; + unsigned int _debug; + // Geometry + player_ranger_geom_t _geom; + player_pose3d_t _sensorPose; + player_bbox3d_t _sensorSize; + // The hardware device itself + std::auto_ptr<gbxsmartbatteryacfr::OceanServer> _device; + // Objects to handle messages from the driver + std::auto_ptr<gbxutilacfr::TrivialTracer> _tracer; +}; + +Driver* +OceanServer_Init (ConfigFile* cf, int section) +{ + return reinterpret_cast <Driver*> (new OceanServer (cf, section)); +} + +void oceanserver_Register(DriverTable* table) +{ + table->AddDriver ("oceanserver", OceanServer_Init); +} + +OceanServer::OceanServer (ConfigFile* cf, int section) + : ThreadedDriver (cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_RANGER_CODE) +{ + // Setup config object + _port = cf->ReadString (section, "port", "/dev/ttyS0"); + _debug = cf->ReadBool (section, "debug", 0); + // Set up geometry information + _geom.pose.px = cf->ReadTupleLength (section, "pose", 0, 0.0f); + _geom.pose.py = cf->ReadTupleLength (section, "pose", 1, 0.0f); + _geom.pose.pz = cf->ReadTupleLength (section, "pose", 2, 0.0f); + _geom.pose.proll = cf->ReadTupleAngle (section, "pose", 3, 0.0f); + _geom.pose.ppitch = cf->ReadTupleAngle (section, "pose", 4, 0.0f); + _geom.pose.pyaw = cf->ReadTupleAngle (section, "pose", 5, 0.0f); + _geom.size.sw = cf->ReadTupleLength (section, "size", 0, 0.0f); + _geom.size.sl = cf->ReadTupleLength (section, "size", 1, 0.0f); + _geom.size.sh = cf->ReadTupleLength (section, "size", 2, 0.0f); + _geom.sensor_poses_count = 1; + _geom.sensor_poses = &_sensorPose; + memcpy (_geom.sensor_poses, &_geom.pose, sizeof (_geom.pose)); + _geom.sensor_sizes_count = 1; + _geom.sensor_sizes = &_sensorSize; + memcpy (_geom.sensor_sizes, &_geom.size, sizeof (_geom.size)); +} + +OceanServer::~OceanServer (void) +{ +} + +int OceanServer::MainSetup (void) +{ + // Create status tracker + _tracer.reset (new gbxutilacfr::TrivialTracer (_debug)); + + // Create the driver object + try + { + _device.reset (new gbxsmartbatteryacfr::OceanServer (_port, *_tracer)); + } + catch (const std::exception& e) + { + PLAYER_ERROR1 ("OceanServer: Failed to initialise device: %s\n", e.what ()); + return -1; + } + + return 0; +} + +void OceanServer::MainQuit (void) +{ + _device.reset (NULL); + _tracer.reset (NULL); +} + +int OceanServer::ProcessMessage (QueuePointer &resp_queue, player_msghdr *hdr, void *data) +{ + // Check for capability requests + HANDLE_CAPABILITY_REQUEST (device_addr, resp_queue, hdr, data, PLAYER_MSGTYPE_REQ, PLAYER_CAPABILTIES_REQ); + return -1; +} + +void OceanServer::Main (void) +{ + while (true) + { + pthread_testcancel (); + ProcessMessages (); + + if (!ReadSensor ()) + break; + } +} + +bool OceanServer::ReadSensor (void) +{ + try + { + gbxsmartbatteryacfr::OceanServerSystem data = _device->getData (); + player_power_data_t powerData; + memset (&powerData, 0, sizeof (powerData)); + + powerData.percent = data.percentCharge; + powerData.valid |= PLAYER_POWER_MASK_PERCENT; + + float lowVoltage = -1; + for (unsigned int ii = 0; ii < 8; ii++) + { + if (!data.availableBatteries[ii]) + continue; + powerData.valid |= PLAYER_POWER_MASK_VOLTS; + if (data.battery (ii).has (gbxsmartbatteryacfr::Voltage)) + { + if (lowVoltage == -1) + lowVoltage = data.battery (ii).voltage (); + else if (data.battery (ii).voltage () < lowVoltage) + lowVoltage = data.battery (ii).voltage (); + } + } + powerData.volts = lowVoltage; + + // First check if any batteries are charging + for (unsigned int ii = 0; ii < 8; ii++) + { + if (!data.availableBatteries[ii]) + continue; + powerData.valid |= PLAYER_POWER_MASK_CHARGING; + if (data.chargingStates[ii]) + powerData.charging = 1; + } + // Next check if any are discharing - because we have to squeeze + // up to 8 batteries into one status, give discharging priority over + // charging. + for (unsigned int ii = 0; ii < 8; ii++) + { + if (!data.availableBatteries[ii]) + continue; + powerData.valid |= PLAYER_POWER_MASK_CHARGING; + if (data.supplyingPowerStates[ii]) + powerData.charging = -1; + } + + Publish (device_addr, PLAYER_MSGTYPE_DATA, PLAYER_POWER_DATA_STATE, reinterpret_cast<void*> (&powerData), sizeof (powerData), NULL); + } + catch (const std::exception &e) + { + PLAYER_ERROR1 ("OceanServer: Failed to read data: %s\n", e.what ()); + return false; + } + + return true; +} + Modified: code/player/trunk/server/drivers/ranger/gbxsickacfr.cc =================================================================== --- code/player/trunk/server/drivers/ranger/gbxsickacfr.cc 2009-06-24 13:21:49 UTC (rev 7887) +++ code/player/trunk/server/drivers/ranger/gbxsickacfr.cc 2009-06-24 13:25:20 UTC (rev 7888) @@ -78,10 +78,10 @@ - size (float 3-tuple: (m, m, m)) - Default: [0.0 0.0 0.0] - Size of the laser in metres. -- retry (integer) + - retry (integer) - Default: 0 - If the initial connection to the laser fails, retry this many times before giving up. -- delay (integer) + - delay (integer) - Default: 0 - Delay (in seconds) before laser is initialized (set this to 32-35 if you have a newer generation Pioneer whose laser is switched on when the serial port is open). This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <gb...@us...> - 2009-07-10 00:47:19
|
Revision: 7970 http://playerstage.svn.sourceforge.net/playerstage/?rev=7970&view=rev Author: gbiggs Date: 2009-07-10 00:47:18 +0000 (Fri, 10 Jul 2009) Log Message: ----------- Added a missing file Modified Paths: -------------- code/player/trunk/server/drivers/camera/1394/CMakeLists.txt Added Paths: ----------- code/player/trunk/server/drivers/vectormap/robotracker.cc Modified: code/player/trunk/server/drivers/camera/1394/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/camera/1394/CMakeLists.txt 2009-07-10 00:39:46 UTC (rev 7969) +++ code/player/trunk/server/drivers/camera/1394/CMakeLists.txt 2009-07-10 00:47:18 UTC (rev 7970) @@ -13,14 +13,14 @@ CHECK_INCLUDE_FILES (libraw1394/raw1394.h haveRaw1394H) IF (haveRaw1394H) SET (c1394LinkFlags "-lraw1394 -ldc1394") - SET (HAVE_LIBRAW1394 1) + SET (HAVE_LIBRAW1394 1) ELSE (haveRaw1394H) SET (c1394LinkFlags "-ldc1394") ENDIF (haveRaw1394H) CHECK_INCLUDE_FILES ("dc1394/dc1394.h;dc1394/linux/control.h" haveLinuxControlH) - IF (haveLinuxControlH) + IF (haveLinuxControlH) SET (DC1394_2_LINUX 1) - ENDIF (haveLinuxControlH) + ENDIF (haveLinuxControlH) ELSE (haveControlH) SET (c1394LinkFlags "-lraw1394 -ldc1394_control") PLAYERDRIVER_REQUIRE_HEADER (camera1394 build_camera1394 libraw1394/raw1394.h) Added: code/player/trunk/server/drivers/vectormap/robotracker.cc =================================================================== --- code/player/trunk/server/drivers/vectormap/robotracker.cc (rev 0) +++ code/player/trunk/server/drivers/vectormap/robotracker.cc 2009-07-10 00:47:18 UTC (rev 7970) @@ -0,0 +1,820 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2004 Brian Gerkey ge...@st... + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +/* + * + * Robot tracker that updates vectormap layer with shapes of given robots. + */ + +/** @ingroup drivers */ +/** @{ */ +/** @defgroup driver_robotracker robotracker + * @brief Robot tracker that updates vectormap layer with shapes of given robots. + +@par Provides + +- @ref interface_opaque + +@par Requires + +- @ref interface_vectormap +- @ref interface_position2d + +@par Configuration requests + +- None + +@par Configuration file options + +- names (string tuple) + - non-empty list of robot names (vectormap layer objects) + +- shape_x (float tuple) + - Default: [ -0.01 0.01 ] + - shape of the robot (x'es of points of linestring) + - all robots are of the same shape + +- shape_y (float tuple) + - Default: [ -0.01 0.01 ] + - shape of the robot (y's of points of linestring) + - all robots are of the same shape + +- interval (float) + - Default: -0.01 (=NONE) + - mimimal interval between map updates + +- min_x_change (float) + - Default: -0.01 (=NONE) + - minimum change on X-axis to assume robot has moved + +- min_y_change (float) + - Default: -0.01 (=NONE) + - minimum change on Y-axis to assume robot has moved + +- layer_name (string) + - Default: "NONE" + - vectormap layer name to be updated + +- workspaces_name (string) + - Default: "NONE" + - if not set to "NONE" this is the name of vectormap layer that will be + filled-up with robots extents + +- depletion_zone (float) + - Default: 0.0 + - length of additional depletion zone in robot workspace + +- first2last_extent_name (string) + - Default: "NONE" + - if not set to "NONE" this is the name of additional vectormap object + that will be added to workspaces layer to denote extent of all robots and + their workspaces + +@par Example + +@verbatim +driver +( + name "robotracker" + names ["roomba0" "roomba1" "roomba2" "roomba3" "roomba4"] + requires ["vectormap:0" "roomba0:::position2d:1" "roomba1::6666:position2d:1" "roomba2::6667:position2d:1" "roomba3::6668:position2d:1" "roomba4::6669:position2d:1"] + provides ["opaque:0"] + shape_x [ 0.225 0.208 0.159 0.086 0.000 -0.086 -0.159 -0.208 -0.225 -0.208 -0.159 -0.086 -0.000 0.086 0.159 0.208 0.225 ] + shape_y [ 0.000 0.086 0.159 0.208 0.225 0.208 0.159 0.086 0.000 -0.086 -0.159 -0.208 -0.225 -0.208 -0.159 -0.086 0.000 ] + min_x_change 0.1 + min_y_change 0.1 + layer_name "obstacles" + workspaces_name "workspaces" + first2last_extent_name "formation_extent" + depletion_zone 0.3 + alwayson 1 +) +@endverbatim + +@author Paul Osmialowski + +*/ + +/** @} */ + +#include <stddef.h> +#include <stdlib.h> +#include <string.h> +#include <strings.h> /* strcasecmp() */ +#include <assert.h> +#include <math.h> +#include <libplayercore/playercore.h> +#include <libplayerwkb/playerwkb.h> + +#define MAX_BOTS 32 +#define MAX_SHAPE_POINTS 64 + +#define EPS 0.00000000000001 + +extern PlayerTime * GlobalTime; + +class RoboTracker : Driver +{ +public: + RoboTracker(ConfigFile * cf, int section); + virtual ~RoboTracker(); + virtual int Setup(); + virtual int Shutdown(); + virtual int ProcessMessage(QueuePointer & resp_queue, + player_msghdr * hdr, + void * data); +private: + playerwkbprocessor_t wkbProcessor; + player_devaddr_t opaque_addr; + player_devaddr_t position_addrs[MAX_BOTS]; + player_devaddr_t vectormap_addr; + bool pos_data_valid[MAX_BOTS]; + bool prev_pos_data_valid[MAX_BOTS]; + player_position2d_data_t pos_data[MAX_BOTS]; + player_position2d_data_t prev_pos_data[MAX_BOTS]; + Device * pos_dev[MAX_BOTS]; + const char * names[MAX_BOTS]; + const char * layer_name; + const char * workspaces_name; + const char * first2last_extent_name; + Device * vectormap_dev; + int shape_points; + double shape[MAX_SHAPE_POINTS][2]; + double extent[5][2]; + double depletion_zone; + int position_devices; + double min_x_change; + double min_y_change; + double interval; + double last_update; + double minx; + double miny; + double maxx; + double maxy; + enum { idle = 0, get_layer_data = 1, write_obstacles = 2, write_workspaces = 3 } state; +}; + +RoboTracker::RoboTracker(ConfigFile * cf, int section) + : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN) +{ + int i; + + this->wkbProcessor = player_wkb_create_processor(); // one per driver instance + memset(&(this->opaque_addr), 0, sizeof(player_devaddr_t)); + memset(this->position_addrs, 0, sizeof this->position_addrs); + memset(&(this->vectormap_addr), 0, sizeof(player_devaddr_t)); + memset(this->pos_data_valid, 0, sizeof this->pos_data_valid); + memset(this->prev_pos_data_valid, 0, sizeof this->prev_pos_data_valid); + memset(this->pos_data, 0, sizeof this->pos_data); + memset(this->prev_pos_data, 0, sizeof this->prev_pos_data); + memset(this->pos_dev, 0, sizeof this->pos_dev); + memset(this->names, 0, sizeof this->names); + this->layer_name = NULL; + this->workspaces_name = NULL; + first2last_extent_name = NULL; + this->vectormap_dev = NULL; + this->shape_points = 0; + memset(this->shape, 0, sizeof this->shape); + memset(this->extent, 0, sizeof this->extent); + this->depletion_zone = 0.0; + this->position_devices = 0; + this->min_x_change = -1.0; + this->min_y_change = -1.0; + this->interval = -1.0; + this->last_update = 0.0; + this->minx = 0.0; + this->miny = 0.0; + this->maxx = 0.0; + this->maxy = 0.0; + this->state = idle; + if (cf->ReadDeviceAddr(&(this->opaque_addr), section, "provides", PLAYER_OPAQUE_CODE, -1, NULL)) + { + this->SetError(-1); + return; + } + if (this->AddInterface(this->opaque_addr)) + { + this->SetError(-1); + return; + } + this->layer_name = cf->ReadString(section, "layer_name", "NONE"); + if (!(this->layer_name)) + { + this->SetError(-1); + return; + } + if (!(strcmp(this->layer_name, "NONE"))) + { + PLAYER_ERROR("layer_name not given"); + this->SetError(-1); + return; + } + this->workspaces_name = cf->ReadString(section, "workspaces_name", "NONE"); + if (!(this->workspaces_name)) + { + this->SetError(-1); + return; + } + this->first2last_extent_name = cf->ReadString(section, "first2last_extent_name", "NONE"); + if (!(this->first2last_extent_name)) + { + this->SetError(-1); + return; + } + this->position_devices = cf->GetTupleCount(section, "names"); + if (((this->position_devices) <= 0) || ((this->position_devices) > MAX_BOTS)) + { + PLAYER_ERROR("invalid number of position devices"); + this->SetError(-1); + return; + } + for (i = 0; i < (this->position_devices); i++) + { + pos_dev[i] = NULL; + pos_data_valid[i] = false; + prev_pos_data_valid[i] = false; + this->names[i] = cf->ReadTupleString(section, "names", i, "NONE"); + if (!(this->names[i])) + { + this->SetError(-1); + return; + } + if (!(strlen(this->names[i]) > 0)) + { + PLAYER_ERROR1("empty names not allowed %d", i); + this->SetError(-1); + return; + } + if (!(strcmp(this->names[i], "NONE"))) + { + PLAYER_ERROR1("name %d not given", i); + this->SetError(-1); + return; + } + if (cf->ReadDeviceAddr(&(this->position_addrs[i]), section, "requires", PLAYER_POSITION2D_CODE, -1, this->names[i])) + { + this->SetError(-1); + return; + } + } + if (cf->ReadDeviceAddr(&(this->vectormap_addr), section, "requires", PLAYER_VECTORMAP_CODE, -1, NULL)) + { + this->SetError(-1); + return; + } + this->shape_points = cf->GetTupleCount(section, "shape_x"); + if ((cf->GetTupleCount(section, "shape_y")) != (this->shape_points)) + { + PLAYER_ERROR("size of shape_x and shape_y sets should be equal"); + this->SetError(-1); + return; + } + if (!((this->shape_points) > 0)) + { + this->minx = -0.01; + this->miny = -0.01; + this->maxx = 0.01; + this->maxy = 0.01; + this->shape_points = 2; + this->shape[0][0] = minx; + this->shape[0][1] = miny; + this->shape[1][0] = maxx; + this->shape[1][1] = maxy; + } else + { + if ((this->shape_points) > MAX_SHAPE_POINTS) + { + PLAYER_ERROR("invalid size of shape_x set"); + this->SetError(-1); + return; + } + for (i = 0; i < (this->shape_points); i++) + { + this->shape[i][0] = cf->ReadTupleFloat(section, "shape_x", i, 0.0); + this->shape[i][1] = cf->ReadTupleFloat(section, "shape_y", i, 0.0); + if (!i) + { + this->minx = this->shape[i][0]; + this->miny = this->shape[i][1]; + this->maxx = this->shape[i][0]; + this->maxy = this->shape[i][1]; + } else + { + if (this->shape[i][0] < this->minx) this->minx = this->shape[i][0]; + if (this->shape[i][1] < this->miny) this->miny = this->shape[i][1]; + if (this->shape[i][0] > this->maxx) this->maxx = this->shape[i][0]; + if (this->shape[i][1] > this->maxy) this->maxy = this->shape[i][1]; + } + } + } + this->depletion_zone = cf->ReadFloat(section, "depletion_zone", 0.0); + this->minx -= this->depletion_zone; + this->miny -= this->depletion_zone; + this->maxx += this->depletion_zone; + this->maxy += this->depletion_zone; + this->extent[0][0] = this->minx; + this->extent[0][1] = this->miny; + this->extent[1][0] = this->maxx; + this->extent[1][1] = this->miny; + this->extent[2][0] = this->maxx; + this->extent[2][1] = this->maxy; + this->extent[3][0] = this->minx; + this->extent[3][1] = this->maxy; + this->extent[4][0] = this->minx; + this->extent[4][1] = this->miny; + this->interval = cf->ReadFloat(section, "interval", -1.0); + this->min_x_change = cf->ReadFloat(section, "min_x_change", -1.0); + this->min_y_change = cf->ReadFloat(section, "min_y_change", -1.0); +} + +RoboTracker::~RoboTracker() +{ + player_wkb_destroy_processor(this->wkbProcessor); +} + +int RoboTracker::Setup() +{ + int i, j; + + memset(this->pos_data, 0, sizeof this->pos_data); + memset(this->prev_pos_data, 0, sizeof this->prev_pos_data); + for (i = 0; i < (this->position_devices); i++) + { + pos_data_valid[i] = false; + prev_pos_data_valid[i] = false; + } + this->last_update = 0.0; + this->state = idle; + this->vectormap_dev = deviceTable->GetDevice(this->vectormap_addr); + if (!(this->vectormap_dev)) + { + PLAYER_ERROR("unable to locate suitable vectormap device"); + return -1; + } + if (this->vectormap_dev->Subscribe(this->InQueue)) + { + PLAYER_ERROR("unable to subscribe to vectormap device"); + this->vectormap_dev = NULL; + return -1; + } + for (i = 0; i < (this->position_devices); i++) + { + this->pos_dev[i] = deviceTable->GetDevice(this->position_addrs[i]); + if (!(this->pos_dev[i])) + { + PLAYER_ERROR1("unable to locate suitable position2d device %d", i); + for (j = 0; j < i; j++) + { + if (this->pos_dev[j]) this->pos_dev[j]->Unsubscribe(this->InQueue); + this->pos_dev[j] = NULL; + } + if (this->vectormap_dev) this->vectormap_dev->Unsubscribe(this->InQueue); + this->vectormap_dev = NULL; + return -1; + } + if (this->pos_dev[i]->Subscribe(this->InQueue)) + { + PLAYER_ERROR1("unable to subscribe to position2d device %d", i); + this->pos_dev[i] = NULL; + for (j = 0; j < i; j++) + { + if (this->pos_dev[j]) this->pos_dev[j]->Unsubscribe(this->InQueue); + this->pos_dev[j] = NULL; + } + if (this->vectormap_dev) this->vectormap_dev->Unsubscribe(this->InQueue); + this->vectormap_dev = NULL; + return -1; + } + } + return 0; +} + +int RoboTracker::Shutdown() +{ + int i; + + for (i = 0; i < (this->position_devices); i++) + { + if (this->pos_dev[i]) this->pos_dev[i]->Unsubscribe(this->InQueue); + this->pos_dev[i] = NULL; + } + if (this->vectormap_dev) this->vectormap_dev->Unsubscribe(this->InQueue); + this->vectormap_dev = NULL; + return 0; +} + +int RoboTracker::ProcessMessage(QueuePointer & resp_queue, player_msghdr * hdr, void * data) +{ + int i, j; + double d; + player_vectormap_layer_data_t layer; + player_vectormap_layer_data_t * layer_data; + player_vectormap_layer_data_t * new_layer_data; + int to_add; + int added; + size_t wkb_size; + double eminx = 0.0; + double eminy = 0.0; + double emaxx = 0.0; + double emaxy = 0.0; + double first2last[5][2]; + player_opaque_data_t opdata; + + assert(hdr); + for (i = 0; i < (this->position_devices); i++) + { + if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_RESP_NACK, -1, this->vectormap_addr)) + { + assert((this->state) != idle); + PLAYER_ERROR("request not accepted by vectormap device"); + this->state = idle; + return 0; + } + if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA, -1, this->position_addrs[i])) + { + assert(data); + if ((this->state) != idle) return 0; + this->pos_data[i] = *(reinterpret_cast<player_position2d_data_t *>(data)); + this->pos_data_valid[i] = true; + for (i = 0; i < (this->position_devices); i++) if (!(this->pos_data_valid[i])) return 0; + for (i = 0; i < (this->position_devices); i++) + { + if (!(this->prev_pos_data_valid[i])) break; + if ((this->min_x_change) > EPS) + { + if (fabs(this->pos_data[i].pos.px - this->prev_pos_data[i].pos.px) >= (this->min_x_change)) break; + } else break; + if ((this->min_y_change) > EPS) + { + if (fabs(this->pos_data[i].pos.py - this->prev_pos_data[i].pos.py) >= (this->min_y_change)) break; + } else break; + } + if (!(i < (this->position_devices))) return 0; + if ((this->interval) > EPS) + { + GlobalTime->GetTimeDouble(&d); + if ((d - (this->last_update)) >= (this->interval)) this->last_update = d; + else return 0; + } + memset(&layer, 0, sizeof layer); + assert(this->layer_name); + layer.name = strdup(this->layer_name); + if (!(layer.name)) + { + PLAYER_ERROR("out of memory"); + return -1; + } + layer.name_count = strlen(layer.name) + 1; + this->vectormap_dev->PutMsg(this->InQueue, PLAYER_MSGTYPE_REQ, PLAYER_VECTORMAP_REQ_GET_LAYER_DATA, reinterpret_cast<void *>(&layer), 0, NULL); + assert(layer.name); + free(layer.name); + this->state = get_layer_data; + return 0; + } + if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_RESP_ACK, -1, this->vectormap_addr)) + { + switch (this->state) + { + case idle: + assert(!"invalid state"); + break; + case get_layer_data: + assert(data); + assert((hdr->subtype) == PLAYER_VECTORMAP_REQ_GET_LAYER_DATA); + layer_data = reinterpret_cast<player_vectormap_layer_data_t *>(data); + if (!(layer_data->name)) + { + PLAYER_ERROR("internal error: no layer name"); + this->state = idle; + return -1; + } + if (strcmp(this->layer_name, layer_data->name)) + { + PLAYER_ERROR("internal error: wrong layer name"); + this->state = idle; + return -1; + } + if ((layer_data->features_count > 0) && (!(layer_data->features))) + { + PLAYER_ERROR("internal error: NULL features"); + this->state = idle; + return -1; + } + to_add = this->position_devices; + for (j = 0; j < static_cast<int>(layer_data->features_count); j++) + { + assert(layer_data->features[j].name); + for (i = 0; i < (this->position_devices); i++) + { + if (!strcasecmp(layer_data->features[j].name, this->names[i])) + { + to_add--; + break; + } + } + } + if (to_add < 0) + { + PLAYER_ERROR("invalid number of the same names"); + this->state = idle; + return -1; + } + new_layer_data = reinterpret_cast<player_vectormap_layer_data_t *>(malloc(sizeof(player_vectormap_layer_data_t))); + if (!new_layer_data) + { + PLAYER_ERROR("out of memory"); + this->state = idle; + return -1; + } + memset(new_layer_data, 0, sizeof(player_vectormap_layer_data_t)); + new_layer_data->name = strdup(this->layer_name); + if (!(new_layer_data->name)) + { + PLAYER_ERROR("out of memory"); + free(new_layer_data); + this->state = idle; + return -1; + } + new_layer_data->name_count = strlen(new_layer_data->name) + 1; + new_layer_data->features_count = layer_data->features_count + to_add; + assert(new_layer_data->features_count > 0); + new_layer_data->features = reinterpret_cast<player_vectormap_feature_data_t *>(malloc(sizeof(player_vectormap_feature_data_t) * (new_layer_data->features_count))); + if (!(new_layer_data->features)) + { + PLAYER_ERROR("out of memory"); + free(new_layer_data->name); + free(new_layer_data); + this->state = idle; + return -1; + } + memset(new_layer_data->features, 0, sizeof(player_vectormap_feature_data_t) * (new_layer_data->features_count)); + added = 0; + for (j = 0; j < static_cast<int>(layer_data->features_count); j++) + { + assert(layer_data->features[j].name); + new_layer_data->features[j].name = strdup(layer_data->features[j].name); + assert(new_layer_data->features[j].name); + new_layer_data->features[j].name_count = strlen(new_layer_data->features[j].name) + 1; + if (layer_data->features[j].attrib) + { + new_layer_data->features[j].attrib = strdup(layer_data->features[j].attrib); + assert(new_layer_data->features[j].attrib); + new_layer_data->features[j].attrib_count = strlen(new_layer_data->features[j].attrib) + 1; + } else + { + new_layer_data->features[j].attrib = reinterpret_cast<char *>(malloc(sizeof(char) * 1)); + assert(new_layer_data->features[j].attrib); + new_layer_data->features[j].attrib[0] = '\0'; + new_layer_data->features[j].attrib_count = 1; + } + for (i = 0; i < (this->position_devices); i++) if (!strcasecmp(layer_data->features[j].name, this->names[i])) break; + if (i < (this->position_devices)) + { + new_layer_data->features[j].wkb_count = player_wkb_create_linestring(this->wkbProcessor, this->shape, this->shape_points, this->pos_data[i].pos.px, this->pos_data[i].pos.py, NULL, 0); + assert(new_layer_data->features[j].wkb_count > 0); + new_layer_data->features[j].wkb = reinterpret_cast<uint8_t *>(malloc(sizeof(uint8_t) * new_layer_data->features[j].wkb_count)); + assert(new_layer_data->features[j].wkb); + wkb_size = player_wkb_create_linestring(this->wkbProcessor, this->shape, this->shape_points, this->pos_data[i].pos.px, this->pos_data[i].pos.py, new_layer_data->features[j].wkb, new_layer_data->features[j].wkb_count); + assert(wkb_size == static_cast<size_t>(new_layer_data->features[j].wkb_count)); + this->prev_pos_data[i] = this->pos_data[i]; + this->pos_data_valid[i] = false; + this->prev_pos_data_valid[i] = true; + added++; + } else + { + assert(layer_data->features[j].wkb); + assert(layer_data->features[j].wkb_count > 0); + new_layer_data->features[j].wkb_count = layer_data->features[j].wkb_count; + new_layer_data->features[j].wkb = reinterpret_cast<uint8_t *>(malloc(sizeof(uint8_t) * new_layer_data->features[j].wkb_count)); + assert(new_layer_data->features[j].wkb); + memcpy(new_layer_data->features[j].wkb, layer_data->features[j].wkb, new_layer_data->features[j].wkb_count); + } + } + layer_data = NULL; + assert(added == ((this->position_devices) - to_add)); + added = 0; + for (i = 0; i < (this->position_devices); i++) + { + if (this->pos_data_valid[i]) + { + assert(j < static_cast<int>(new_layer_data->features_count)); + new_layer_data->features[j].name = strdup(this->names[i]); + assert(new_layer_data->features[j].name); + new_layer_data->features[j].name_count = strlen(new_layer_data->features[j].name) + 1; + new_layer_data->features[j].attrib = reinterpret_cast<char *>(malloc(sizeof(char) * 1)); + assert(new_layer_data->features[j].attrib); + new_layer_data->features[j].attrib[0] = '\0'; + new_layer_data->features[j].attrib_count = 1; + new_layer_data->features[j].wkb_count = player_wkb_create_linestring(this->wkbProcessor, this->shape, this->shape_points, this->pos_data[i].pos.px, this->pos_data[i].pos.py, NULL, 0); + assert(new_layer_data->features[j].wkb_count > 0); + new_layer_data->features[j].wkb = reinterpret_cast<uint8_t *>(malloc(sizeof(uint8_t) * new_layer_data->features[j].wkb_count)); + assert(new_layer_data->features[j].wkb); + wkb_size = player_wkb_create_linestring(this->wkbProcessor, this->shape, this->shape_points, this->pos_data[i].pos.px, this->pos_data[i].pos.py, new_layer_data->features[j].wkb, new_layer_data->features[j].wkb_count); + assert(wkb_size == static_cast<size_t>(new_layer_data->features[j].wkb_count)); + this->prev_pos_data[i] = this->pos_data[i]; + this->pos_data_valid[i] = false; + this->prev_pos_data_valid[i] = true; + added++; j++; + } + } + assert(added == to_add); + this->vectormap_dev->PutMsg(this->InQueue, PLAYER_MSGTYPE_REQ, PLAYER_VECTORMAP_REQ_WRITE_LAYER, reinterpret_cast<void *>(new_layer_data), 0, NULL); + assert(new_layer_data->name); + free(new_layer_data->name); + assert(new_layer_data->features); + assert(new_layer_data->features_count > 0); + for (j = 0; j < static_cast<int>(new_layer_data->features_count); j++) + { + assert(new_layer_data->features[j].name); + free(new_layer_data->features[j].name); + assert(new_layer_data->features[j].attrib); + free(new_layer_data->features[j].attrib); + assert(new_layer_data->features[j].wkb); + free(new_layer_data->features[j].wkb); + } + free(new_layer_data->features); + free(new_layer_data); + new_layer_data = NULL; + this->state = write_obstacles; + break; + case write_obstacles: + assert((hdr->subtype) == PLAYER_VECTORMAP_REQ_WRITE_LAYER); + assert(this->workspaces_name); + if (!(strcmp(this->workspaces_name, "NONE"))) + { + memset(&opdata, 0, sizeof opdata); + opdata.data_count = 0; + opdata.data = NULL; + this->Publish(this->opaque_addr, + PLAYER_MSGTYPE_DATA, PLAYER_OPAQUE_DATA_STATE, + reinterpret_cast<void *>(&opdata), 0, NULL, true); // copy = true, do not dispose data placed on local stack! + this->state = idle; + break; + } + new_layer_data = reinterpret_cast<player_vectormap_layer_data_t *>(malloc(sizeof(player_vectormap_layer_data_t))); + if (!new_layer_data) + { + PLAYER_ERROR("out of memory"); + this->state = idle; + return -1; + } + memset(new_layer_data, 0, sizeof(player_vectormap_layer_data_t)); + new_layer_data->name = strdup(this->workspaces_name); + if (!(new_layer_data->name)) + { + PLAYER_ERROR("out of memory"); + free(new_layer_data); + this->state = idle; + return -1; + } + new_layer_data->name_count = strlen(new_layer_data->name) + 1; + new_layer_data->features_count = this->position_devices; + assert(this->first2last_extent_name); + if (strcmp(this->first2last_extent_name, "NONE")) (new_layer_data->features_count)++; + assert(new_layer_data->features_count > 0); + new_layer_data->features = reinterpret_cast<player_vectormap_feature_data_t *>(malloc(sizeof(player_vectormap_feature_data_t) * (new_layer_data->features_count))); + if (!(new_layer_data->features)) + { + PLAYER_ERROR("out of memory"); + free(new_layer_data->name); + free(new_layer_data); + this->state = idle; + return -1; + } + memset(new_layer_data->features, 0, sizeof(player_vectormap_feature_data_t) * (new_layer_data->features_count)); + for (i = 0; i < (this->position_devices); i++) + { + assert(this->prev_pos_data_valid[i]); + assert(i < static_cast<int>(new_layer_data->features_count)); + if (!i) + { + eminx = this->prev_pos_data[i].pos.px; + eminy = this->prev_pos_data[i].pos.py; + emaxx = this->prev_pos_data[i].pos.px; + emaxy = this->prev_pos_data[i].pos.py; + } else + { + if (this->prev_pos_data[i].pos.px < eminx) eminx = this->prev_pos_data[i].pos.px; + if (this->prev_pos_data[i].pos.py < eminy) eminy = this->prev_pos_data[i].pos.py; + if (this->prev_pos_data[i].pos.px > emaxx) emaxx = this->prev_pos_data[i].pos.px; + if (this->prev_pos_data[i].pos.py > emaxy) emaxy = this->prev_pos_data[i].pos.py; + } + new_layer_data->features[i].name = strdup(this->names[i]); + assert(new_layer_data->features[i].name); + new_layer_data->features[i].name_count = strlen(new_layer_data->features[i].name) + 1; + new_layer_data->features[i].attrib = reinterpret_cast<char *>(malloc(sizeof(char) * 1)); + assert(new_layer_data->features[i].attrib); + new_layer_data->features[i].attrib[0] = '\0'; + new_layer_data->features[i].attrib_count = 1; + new_layer_data->features[i].wkb_count = player_wkb_create_linestring(this->wkbProcessor, this->extent, 5, this->prev_pos_data[i].pos.px, this->prev_pos_data[i].pos.py, NULL, 0); + assert(new_layer_data->features[i].wkb_count > 0); + new_layer_data->features[i].wkb = reinterpret_cast<uint8_t *>(malloc(sizeof(uint8_t) * new_layer_data->features[i].wkb_count)); + assert(new_layer_data->features[i].wkb); + wkb_size = player_wkb_create_linestring(this->wkbProcessor, this->extent, 5, this->pos_data[i].pos.px, this->pos_data[i].pos.py, new_layer_data->features[i].wkb, new_layer_data->features[i].wkb_count); + assert(wkb_size == static_cast<size_t>(new_layer_data->features[i].wkb_count)); + } + eminx += this->minx; + eminy += this->miny; + emaxx += this->maxx; + emaxy += this->maxy; + first2last[0][0] = eminx; + first2last[0][1] = eminy; + first2last[1][0] = emaxx; + first2last[1][1] = eminy; + first2last[2][0] = emaxx; + first2last[2][1] = emaxy; + first2last[3][0] = eminx; + first2last[3][1] = emaxy; + first2last[4][0] = eminx; + first2last[4][1] = eminy; + assert(this->first2last_extent_name); + if (strcmp(this->first2last_extent_name, "NONE")) + { + assert(i < static_cast<int>(new_layer_data->features_count)); + new_layer_data->features[i].name = strdup(this->first2last_extent_name); + assert(new_layer_data->features[i].name); + new_layer_data->features[i].name_count = strlen(new_layer_data->features[i].name) + 1; + new_layer_data->features[i].attrib = reinterpret_cast<char *>(malloc(sizeof(char) * 1)); + assert(new_layer_data->features[i].attrib); + new_layer_data->features[i].attrib[0] = '\0'; + new_layer_data->features[i].attrib_count = 1; + new_layer_data->features[i].wkb_count = player_wkb_create_linestring(this->wkbProcessor, first2last, 5, 0.0, 0.0, NULL, 0); + assert(new_layer_data->features[i].wkb_count > 0); + new_layer_data->features[i].wkb = reinterpret_cast<uint8_t *>(malloc(sizeof(uint8_t) * new_layer_data->features[i].wkb_count)); + assert(new_layer_data->features[i].wkb); + wkb_size = player_wkb_create_linestring(this->wkbProcessor, first2last, 5, 0.0, 0.0, new_layer_data->features[i].wkb, new_layer_data->features[i].wkb_count); + assert(wkb_size == static_cast<size_t>(new_layer_data->features[i].wkb_count)); + } + this->vectormap_dev->PutMsg(this->InQueue, PLAYER_MSGTYPE_REQ, PLAYER_VECTORMAP_REQ_WRITE_LAYER, reinterpret_cast<void *>(new_layer_data), 0, NULL); + assert(new_layer_data->name); + free(new_layer_data->name); + assert(new_layer_data->features); + assert(new_layer_data->features_count > 0); + for (j = 0; j < static_cast<int>(new_layer_data->features_count); j++) + { + assert(new_layer_data->features[j].name); + free(new_layer_data->features[j].name); + assert(new_layer_data->features[j].attrib); + free(new_layer_data->features[j].attrib); + assert(new_layer_data->features[j].wkb); + free(new_layer_data->features[j].wkb); + } + free(new_layer_data->features); + free(new_layer_data); + new_layer_data = NULL; + this->state = write_workspaces; + break; + case write_workspaces: + assert((hdr->subtype) == PLAYER_VECTORMAP_REQ_WRITE_LAYER); + memset(&opdata, 0, sizeof opdata); + opdata.data_count = 0; + opdata.data = NULL; + this->Publish(this->opaque_addr, + PLAYER_MSGTYPE_DATA, PLAYER_OPAQUE_DATA_STATE, + reinterpret_cast<void *>(&opdata), 0, NULL, true); // copy = true, do not dispose data placed on local stack! + this->state = idle; + break; + default: + assert(!"unknown state"); + } + return 0; + } + } + return -1; +} + +// A factory creation function, declared outside of the class so that it +// can be invoked without any object context (alternatively, you can +// declare it static in the class). In this function, we create and return +// (as a generic Driver*) a pointer to a new instance of this driver. +Driver * RoboTracker_Init(ConfigFile * cf, int section) +{ + // Create and return a new instance of this driver + return reinterpret_cast<Driver *>(new RoboTracker(cf, section)); +} + +// A driver registration function, again declared outside of the class so +// that it can be invoked without object context. In this function, we add +// the driver into the given driver table, indicating which interface the +// driver can support and how to create a driver instance. +void robotracker_Register(DriverTable * table) +{ + table->AddDriver("robotracker", RoboTracker_Init); +} This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <gb...@us...> - 2009-07-13 23:34:30
|
Revision: 8009 http://playerstage.svn.sourceforge.net/playerstage/?rev=8009&view=rev Author: gbiggs Date: 2009-07-13 23:34:19 +0000 (Mon, 13 Jul 2009) Log Message: ----------- Applied patch #2820915 Modified Paths: -------------- code/player/trunk/server/drivers/laser/sicklms200.cc code/player/trunk/server/drivers/ptz/sonyevid30.cc Modified: code/player/trunk/server/drivers/laser/sicklms200.cc =================================================================== --- code/player/trunk/server/drivers/laser/sicklms200.cc 2009-07-13 23:00:51 UTC (rev 8008) +++ code/player/trunk/server/drivers/laser/sicklms200.cc 2009-07-13 23:34:19 UTC (rev 8009) @@ -910,7 +910,11 @@ // Returns 0 on success int SickLMS200::OpenTerm() { +#if defined (__QNXNTO__) + this->laser_fd = ::open(this->device_name, O_RDWR , S_IRUSR | S_IWUSR ); +#else this->laser_fd = ::open(this->device_name, O_RDWR | O_SYNC , S_IRUSR | S_IWUSR ); +#endif if (this->laser_fd < 0) { PLAYER_ERROR2("unable to open serial port [%s]; [%s]", Modified: code/player/trunk/server/drivers/ptz/sonyevid30.cc =================================================================== --- code/player/trunk/server/drivers/ptz/sonyevid30.cc 2009-07-13 23:00:51 UTC (rev 8008) +++ code/player/trunk/server/drivers/ptz/sonyevid30.cc 2009-07-13 23:34:19 UTC (rev 8009) @@ -604,7 +604,11 @@ fflush(stdout); // open it. non-blocking at first, in case there's no ptz unit. +#if defined (__QNXNTO__) + if((ptz_fd = open(ptz_serial_port, O_RDWR | O_NONBLOCK, S_IRUSR | S_IWUSR )) < 0 ) +#else if((ptz_fd = open(ptz_serial_port, O_RDWR | O_SYNC | O_NONBLOCK, S_IRUSR | S_IWUSR )) < 0 ) +#endif { perror("SonyEVID30::Setup():open():"); return(-1); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <gb...@us...> - 2009-07-21 05:03:48
|
Revision: 8063 http://playerstage.svn.sourceforge.net/playerstage/?rev=8063&view=rev Author: gbiggs Date: 2009-07-21 05:03:39 +0000 (Tue, 21 Jul 2009) Log Message: ----------- Re-enabled drivers on windows Modified Paths: -------------- code/player/trunk/server/drivers/position/globalize/CMakeLists.txt code/player/trunk/server/drivers/position/goto/CMakeLists.txt code/player/trunk/server/drivers/position/motionmind/CMakeLists.txt code/player/trunk/server/drivers/vectormap/CMakeLists.txt Modified: code/player/trunk/server/drivers/position/globalize/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/position/globalize/CMakeLists.txt 2009-07-21 03:47:16 UTC (rev 8062) +++ code/player/trunk/server/drivers/position/globalize/CMakeLists.txt 2009-07-21 05:03:39 UTC (rev 8063) @@ -1,5 +1,2 @@ PLAYERDRIVER_OPTION (globalize build_globalize ON) -# This driver uses GlobalTime from libplayercore, which currently doesn't -# get exported correctly. -PLAYERDRIVER_REJECT_OS (globalize build_globalize PLAYER_OS_WIN) PLAYERDRIVER_ADD_DRIVER (globalize build_globalize SOURCES globalize.cc) Modified: code/player/trunk/server/drivers/position/goto/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/position/goto/CMakeLists.txt 2009-07-21 03:47:16 UTC (rev 8062) +++ code/player/trunk/server/drivers/position/goto/CMakeLists.txt 2009-07-21 05:03:39 UTC (rev 8063) @@ -1,5 +1,2 @@ PLAYERDRIVER_OPTION (goto build_goto ON) -# This driver uses GlobalTime from libplayercore, which currently doesn't -# get exported correctly. -PLAYERDRIVER_REJECT_OS (goto build_goto PLAYER_OS_WIN) PLAYERDRIVER_ADD_DRIVER (goto build_goto SOURCES goto.cc) Modified: code/player/trunk/server/drivers/position/motionmind/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/position/motionmind/CMakeLists.txt 2009-07-21 03:47:16 UTC (rev 8062) +++ code/player/trunk/server/drivers/position/motionmind/CMakeLists.txt 2009-07-21 05:03:39 UTC (rev 8063) @@ -1,6 +1,3 @@ SET (motionmindSrcs motionmind.cc) PLAYERDRIVER_OPTION (motionmind build_motionmind ON) -# This driver uses GlobalTime from libplayercore, which currently doesn't -# get exported correctly. -PLAYERDRIVER_REJECT_OS (motionmind build_motionmind PLAYER_OS_WIN) PLAYERDRIVER_ADD_DRIVER (motionmind build_motionmind SOURCES ${motionmindSrcs}) Modified: code/player/trunk/server/drivers/vectormap/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/vectormap/CMakeLists.txt 2009-07-21 03:47:16 UTC (rev 8062) +++ code/player/trunk/server/drivers/vectormap/CMakeLists.txt 2009-07-21 05:03:39 UTC (rev 8063) @@ -19,7 +19,4 @@ PLAYERDRIVER_ADD_DRIVER (vec2map build_vec2map SOURCES vec2map.cc) PLAYERDRIVER_OPTION (robotracker build_robotracker ON) -# This driver uses GlobalTime from libplayercore, which currently doesn't -# get exported correctly. -PLAYERDRIVER_REJECT_OS (robotracker build_robotracker PLAYER_OS_WIN) PLAYERDRIVER_ADD_DRIVER (robotracker build_robotracker SOURCES robotracker.cc) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <gb...@us...> - 2009-07-21 22:24:59
|
Revision: 8069 http://playerstage.svn.sourceforge.net/playerstage/?rev=8069&view=rev Author: gbiggs Date: 2009-07-21 22:24:52 +0000 (Tue, 21 Jul 2009) Log Message: ----------- Added version checks for some libs Modified Paths: -------------- code/player/trunk/server/drivers/gps/CMakeLists.txt code/player/trunk/server/drivers/opaque/CMakeLists.txt code/player/trunk/server/drivers/power/CMakeLists.txt code/player/trunk/server/drivers/ranger/CMakeLists.txt Modified: code/player/trunk/server/drivers/gps/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/gps/CMakeLists.txt 2009-07-21 21:58:57 UTC (rev 8068) +++ code/player/trunk/server/drivers/gps/CMakeLists.txt 2009-07-21 22:24:52 UTC (rev 8069) @@ -5,7 +5,7 @@ PLAYERDRIVER_OPTION (gbxgarminacfr build_gbxgarminacfr ON) PLAYERDRIVER_REQUIRE_PKG (gbxgarminacfr build_gbxgarminacfr GbxGarminAcfr gbxgarminacfr_includeDirs gbxgarminacfr_libDirs gbxgarminacfr_linkLibs - gbxgarminacfr_linkFlags gbxgarminacfr_cFlags) + gbxgarminacfr_linkFlags gbxgarminacfr_cFlags 1.0.0) PLAYERDRIVER_ADD_DRIVER (gbxgarminacfr build_gbxgarminacfr INCLUDEDIRS ${gbxgarminacfr_includeDirs} LIBDIRS ${gbxgarminacfr_libDirs} LINKLIBS ${gbxgarminacfr_linkLibs} LINKFLAGS ${gbxgarminacfr_linkFlags} Modified: code/player/trunk/server/drivers/opaque/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/opaque/CMakeLists.txt 2009-07-21 21:58:57 UTC (rev 8068) +++ code/player/trunk/server/drivers/opaque/CMakeLists.txt 2009-07-21 22:24:52 UTC (rev 8069) @@ -1,6 +1,6 @@ PLAYERDRIVER_OPTION (flexiport build_flexiport ON) PLAYERDRIVER_REQUIRE_PKG (flexiport build_flexiport flexiport flexiport_includeDirs - flexiport_libDirs flexiport_linkLibs flexiport_linkFlags flexiport_cFlags) + flexiport_libDirs flexiport_linkLibs flexiport_linkFlags flexiport_cFlags 1.0.0) PLAYERDRIVER_ADD_DRIVER (flexiport build_flexiport INCLUDEDIRS ${flexiport_includeDirs} LIBDIRS ${flexiport_libDirs} LINKLIBS ${flexiport_linkLibs} LINKFLAGS ${flexiport_linkFlags} CFLAGS ${flexiport_cFlags} Modified: code/player/trunk/server/drivers/power/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/power/CMakeLists.txt 2009-07-21 21:58:57 UTC (rev 8068) +++ code/player/trunk/server/drivers/power/CMakeLists.txt 2009-07-21 22:24:52 UTC (rev 8069) @@ -1,7 +1,7 @@ PLAYERDRIVER_OPTION (oceanserver build_oceanserver ON) PLAYERDRIVER_REQUIRE_PKG (oceanserver build_oceanserver GbxSmartBatteryAcfr gbxsmartbatteryacfr_includeDirs gbxsmartbatteryacfr_libDirs gbxsmartbatteryacfr_linkLibs - gbxsmartbatteryacfr_linkFlags gbxsmartbatteryacfr_cFlags) + gbxsmartbatteryacfr_linkFlags gbxsmartbatteryacfr_cFlags 1.0.0) PLAYERDRIVER_ADD_DRIVER (oceanserver build_oceanserver INCLUDEDIRS ${gbxsmartbatteryacfr_includeDirs} LIBDIRS ${gbxsmartbatteryacfr_libDirs} LINKLIBS ${gbxsmartbatteryacfr_linkLibs} LINKFLAGS ${gbxsmartbatteryacfr_linkFlags} Modified: code/player/trunk/server/drivers/ranger/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/ranger/CMakeLists.txt 2009-07-21 21:58:57 UTC (rev 8068) +++ code/player/trunk/server/drivers/ranger/CMakeLists.txt 2009-07-21 22:24:52 UTC (rev 8069) @@ -1,7 +1,7 @@ PLAYERDRIVER_OPTION (gbxsickacfr build_gbxsickacfr ON) PLAYERDRIVER_REQUIRE_PKG (gbxsickacfr build_gbxsickacfr GbxSickAcfr gbxsickacfr_includeDirs gbxsickacfr_libDirs gbxsickacfr_linkLibs - gbxsickacfr_linkFlags gbxsickacfr_cFlags) + gbxsickacfr_linkFlags gbxsickacfr_cFlags 1.0.0) PLAYERDRIVER_ADD_DRIVER (gbxsickacfr build_gbxsickacfr INCLUDEDIRS ${gbxsickacfr_includeDirs} LIBDIRS ${gbxsickacfr_libDirs} LINKLIBS ${gbxsickacfr_linkLibs} LINKFLAGS ${gbxsickacfr_linkFlags} @@ -10,7 +10,7 @@ PLAYERDRIVER_OPTION (hokuyo_aist build_hokuyo_aist ON) PLAYERDRIVER_REQUIRE_PKG (hokuyo_aist build_hokuyo_aist hokuyo_aist hokuyo_aist_includeDirs hokuyo_aist_libDirs hokuyo_aist_linkLibs - hokuyo_aist_linkFlags hokuyo_aist_cFlags) + hokuyo_aist_linkFlags hokuyo_aist_cFlags 1.0.0) PLAYERDRIVER_ADD_DRIVER (hokuyo_aist build_hokuyo_aist INCLUDEDIRS ${hokuyo_aist_includeDirs} LIBDIRS ${hokuyo_aist_libDirs} LINKLIBS ${hokuyo_aist_linkLibs} LINKFLAGS ${hokuyo_aist_linkFlags} This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2009-07-23 15:28:52
|
Revision: 8093 http://playerstage.svn.sourceforge.net/playerstage/?rev=8093&view=rev Author: thjc Date: 2009-07-23 15:28:48 +0000 (Thu, 23 Jul 2009) Log Message: ----------- more copyright info for drivers I added Modified Paths: -------------- code/player/trunk/server/drivers/mixed/khepera/khepera_serial.cc code/player/trunk/server/drivers/mixed/khepera/khepera_serial.h code/player/trunk/server/drivers/position/ascension/flockofbirds.cc code/player/trunk/server/drivers/position/bumpersafe/bumpersafe.cc code/player/trunk/server/drivers/position/lasersafe/lasersafe.cc Modified: code/player/trunk/server/drivers/mixed/khepera/khepera_serial.cc =================================================================== --- code/player/trunk/server/drivers/mixed/khepera/khepera_serial.cc 2009-07-23 15:17:06 UTC (rev 8092) +++ code/player/trunk/server/drivers/mixed/khepera/khepera_serial.cc 2009-07-23 15:28:48 UTC (rev 8093) @@ -1,3 +1,24 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2006 + * Toby Collett + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + #include "khepera_serial.h" #include <sys/types.h> Modified: code/player/trunk/server/drivers/mixed/khepera/khepera_serial.h =================================================================== --- code/player/trunk/server/drivers/mixed/khepera/khepera_serial.h 2009-07-23 15:17:06 UTC (rev 8092) +++ code/player/trunk/server/drivers/mixed/khepera/khepera_serial.h 2009-07-23 15:28:48 UTC (rev 8093) @@ -1,3 +1,24 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2006 + * Toby Collett + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + #ifndef KHEPERA_SERIAL_H #define KHEPERA_SERIAL_H Modified: code/player/trunk/server/drivers/position/ascension/flockofbirds.cc =================================================================== --- code/player/trunk/server/drivers/position/ascension/flockofbirds.cc 2009-07-23 15:17:06 UTC (rev 8092) +++ code/player/trunk/server/drivers/position/ascension/flockofbirds.cc 2009-07-23 15:28:48 UTC (rev 8093) @@ -1,3 +1,24 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2006 + * Toby Collett + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + /** @ingroup drivers */ /** @{ */ /** @defgroup driver_flockofbirds flockofbirds Modified: code/player/trunk/server/drivers/position/bumpersafe/bumpersafe.cc =================================================================== --- code/player/trunk/server/drivers/position/bumpersafe/bumpersafe.cc 2009-07-23 15:17:06 UTC (rev 8092) +++ code/player/trunk/server/drivers/position/bumpersafe/bumpersafe.cc 2009-07-23 15:28:48 UTC (rev 8093) @@ -1,3 +1,24 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2006 + * Toby Collett + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + /** @ingroup drivers */ /** @{ */ /** @defgroup driver_bumpersafe bumpersafe Modified: code/player/trunk/server/drivers/position/lasersafe/lasersafe.cc =================================================================== --- code/player/trunk/server/drivers/position/lasersafe/lasersafe.cc 2009-07-23 15:17:06 UTC (rev 8092) +++ code/player/trunk/server/drivers/position/lasersafe/lasersafe.cc 2009-07-23 15:28:48 UTC (rev 8093) @@ -1,3 +1,24 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2006 + * Toby Collett + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + /** @ingroup drivers */ /** @{ */ /** @defgroup driver_lasersafe lasersafe This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2009-07-23 23:03:51
|
Revision: 8108 http://playerstage.svn.sourceforge.net/playerstage/?rev=8108&view=rev Author: thjc Date: 2009-07-23 23:03:37 +0000 (Thu, 23 Jul 2009) Log Message: ----------- updating files with copyright headers as per info from Brian Gerkey Modified Paths: -------------- code/player/trunk/server/drivers/localization/amcl/map/map.c code/player/trunk/server/drivers/localization/amcl/map/map.h code/player/trunk/server/drivers/localization/amcl/map/map_draw.c code/player/trunk/server/drivers/localization/amcl/map/map_range.c code/player/trunk/server/drivers/localization/amcl/map/map_store.c code/player/trunk/server/drivers/localization/amcl/models/laser.h code/player/trunk/server/drivers/localization/amcl/models/odometry.h code/player/trunk/server/drivers/localization/amcl/models/sonar.h code/player/trunk/server/drivers/localization/amcl/pf/eig3.c code/player/trunk/server/drivers/localization/amcl/pf/eig3.h code/player/trunk/server/drivers/localization/amcl/pf/pf.c code/player/trunk/server/drivers/localization/amcl/pf/pf.h code/player/trunk/server/drivers/localization/amcl/pf/pf_draw.c code/player/trunk/server/drivers/localization/amcl/pf/pf_kdtree.c code/player/trunk/server/drivers/localization/amcl/pf/pf_kdtree.h code/player/trunk/server/drivers/localization/amcl/pf/pf_pdf.c code/player/trunk/server/drivers/localization/amcl/pf/pf_pdf.h code/player/trunk/server/drivers/localization/amcl/pf/pf_vector.c code/player/trunk/server/drivers/localization/amcl/pf/pf_vector.h code/player/trunk/server/drivers/mixed/irobot/roomba/test.c code/player/trunk/server/drivers/mixed/rmp/canio.h code/player/trunk/server/drivers/mixed/rmp/canio_kvaser.cc code/player/trunk/server/drivers/mixed/rmp/canio_kvaser.h code/player/trunk/server/drivers/planner/wavefront/plan.c code/player/trunk/server/drivers/planner/wavefront/plan.h code/player/trunk/server/drivers/planner/wavefront/plan_control.c code/player/trunk/server/drivers/planner/wavefront/plan_plan.c code/player/trunk/server/drivers/planner/wavefront/plan_waypoint.c code/player/trunk/server/drivers/planner/wavefront/test.c code/player/trunk/server/drivers/position/vfh/vfh.cc code/player/trunk/server/drivers/shell/readlog_time.cc code/player/trunk/server/drivers/shell/readlog_time.h Modified: code/player/trunk/server/drivers/localization/amcl/map/map.c =================================================================== --- code/player/trunk/server/drivers/localization/amcl/map/map.c 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/localization/amcl/map/map.c 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,4 +1,26 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Andrew Howard + * Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /************************************************************************** * Desc: Global map (grid-based) * Author: Andrew Howard Modified: code/player/trunk/server/drivers/localization/amcl/map/map.h =================================================================== --- code/player/trunk/server/drivers/localization/amcl/map/map.h 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/localization/amcl/map/map.h 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,4 +1,26 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Andrew Howard + * Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /************************************************************************** * Desc: Global map (grid-based) * Author: Andrew Howard Modified: code/player/trunk/server/drivers/localization/amcl/map/map_draw.c =================================================================== --- code/player/trunk/server/drivers/localization/amcl/map/map_draw.c 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/localization/amcl/map/map_draw.c 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,4 +1,26 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Andrew Howard + * Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /************************************************************************** * Desc: Local map GUI functions * Author: Andrew Howard Modified: code/player/trunk/server/drivers/localization/amcl/map/map_range.c =================================================================== --- code/player/trunk/server/drivers/localization/amcl/map/map_range.c 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/localization/amcl/map/map_range.c 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,4 +1,26 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Andrew Howard + * Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /************************************************************************** * Desc: Range routines * Author: Andrew Howard Modified: code/player/trunk/server/drivers/localization/amcl/map/map_store.c =================================================================== --- code/player/trunk/server/drivers/localization/amcl/map/map_store.c 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/localization/amcl/map/map_store.c 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,4 +1,26 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Andrew Howard + * Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /************************************************************************** * Desc: Global map storage functions * Author: Andrew Howard Modified: code/player/trunk/server/drivers/localization/amcl/models/laser.h =================================================================== --- code/player/trunk/server/drivers/localization/amcl/models/laser.h 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/localization/amcl/models/laser.h 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,4 +1,26 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Andrew Howard + * Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /************************************************************************** * Desc: Sensor models for the laser sensor. * Author: Andrew Howard Modified: code/player/trunk/server/drivers/localization/amcl/models/odometry.h =================================================================== --- code/player/trunk/server/drivers/localization/amcl/models/odometry.h 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/localization/amcl/models/odometry.h 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,4 +1,26 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Andrew Howard + * Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /************************************************************************** * Desc: Sensor/action models for odometry. * Author: Andrew Howard Modified: code/player/trunk/server/drivers/localization/amcl/models/sonar.h =================================================================== --- code/player/trunk/server/drivers/localization/amcl/models/sonar.h 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/localization/amcl/models/sonar.h 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,4 +1,26 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Andrew Howard + * Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /************************************************************************** * Desc: Sensor models for the sonar sensor. * Author: Andrew Howard Modified: code/player/trunk/server/drivers/localization/amcl/pf/eig3.c =================================================================== --- code/player/trunk/server/drivers/localization/amcl/pf/eig3.c 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/localization/amcl/pf/eig3.c 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,4 +1,26 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Andrew Howard + * Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /* Eigen decomposition code for symmetric 3x3 matrices, copied from the public domain Java Matrix library JAMA. */ Modified: code/player/trunk/server/drivers/localization/amcl/pf/eig3.h =================================================================== --- code/player/trunk/server/drivers/localization/amcl/pf/eig3.h 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/localization/amcl/pf/eig3.h 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,4 +1,26 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Andrew Howard + * Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /* Eigen-decomposition for symmetric 3x3 real matrices. Public domain, copied from the public domain Java library JAMA. */ Modified: code/player/trunk/server/drivers/localization/amcl/pf/pf.c =================================================================== --- code/player/trunk/server/drivers/localization/amcl/pf/pf.c 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/localization/amcl/pf/pf.c 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,4 +1,26 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Andrew Howard + * Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /************************************************************************** * Desc: Simple particle filter for localization. * Author: Andrew Howard Modified: code/player/trunk/server/drivers/localization/amcl/pf/pf.h =================================================================== --- code/player/trunk/server/drivers/localization/amcl/pf/pf.h 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/localization/amcl/pf/pf.h 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,4 +1,26 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Andrew Howard + * Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /************************************************************************** * Desc: Simple particle filter for localization. * Author: Andrew Howard Modified: code/player/trunk/server/drivers/localization/amcl/pf/pf_draw.c =================================================================== --- code/player/trunk/server/drivers/localization/amcl/pf/pf_draw.c 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/localization/amcl/pf/pf_draw.c 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,4 +1,26 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Andrew Howard + * Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /************************************************************************** * Desc: Particle filter; drawing routines * Author: Andrew Howard Modified: code/player/trunk/server/drivers/localization/amcl/pf/pf_kdtree.c =================================================================== --- code/player/trunk/server/drivers/localization/amcl/pf/pf_kdtree.c 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/localization/amcl/pf/pf_kdtree.c 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,4 +1,26 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Andrew Howard + * Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /************************************************************************** * Desc: kd-tree functions * Author: Andrew Howard Modified: code/player/trunk/server/drivers/localization/amcl/pf/pf_kdtree.h =================================================================== --- code/player/trunk/server/drivers/localization/amcl/pf/pf_kdtree.h 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/localization/amcl/pf/pf_kdtree.h 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,4 +1,26 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Andrew Howard + * Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /************************************************************************** * Desc: KD tree functions * Author: Andrew Howard Modified: code/player/trunk/server/drivers/localization/amcl/pf/pf_pdf.c =================================================================== --- code/player/trunk/server/drivers/localization/amcl/pf/pf_pdf.c 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/localization/amcl/pf/pf_pdf.c 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,4 +1,26 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Andrew Howard + * Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /************************************************************************** * Desc: Useful pdf functions * Author: Andrew Howard Modified: code/player/trunk/server/drivers/localization/amcl/pf/pf_pdf.h =================================================================== --- code/player/trunk/server/drivers/localization/amcl/pf/pf_pdf.h 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/localization/amcl/pf/pf_pdf.h 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,4 +1,26 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Andrew Howard + * Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /************************************************************************** * Desc: Useful pdf functions * Author: Andrew Howard Modified: code/player/trunk/server/drivers/localization/amcl/pf/pf_vector.c =================================================================== --- code/player/trunk/server/drivers/localization/amcl/pf/pf_vector.c 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/localization/amcl/pf/pf_vector.c 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,4 +1,26 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Andrew Howard + * Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /************************************************************************** * Desc: Vector functions * Author: Andrew Howard Modified: code/player/trunk/server/drivers/localization/amcl/pf/pf_vector.h =================================================================== --- code/player/trunk/server/drivers/localization/amcl/pf/pf_vector.h 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/localization/amcl/pf/pf_vector.h 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,4 +1,26 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Andrew Howard + * Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /************************************************************************** * Desc: Vector functions * Author: Andrew Howard Modified: code/player/trunk/server/drivers/mixed/irobot/roomba/test.c =================================================================== --- code/player/trunk/server/drivers/mixed/irobot/roomba/test.c 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/mixed/irobot/roomba/test.c 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,3 +1,25 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2006 - + * Brian Gerkey + * + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + #include <stdio.h> #include <stdlib.h> #include <unistd.h> Modified: code/player/trunk/server/drivers/mixed/rmp/canio.h =================================================================== --- code/player/trunk/server/drivers/mixed/rmp/canio.h 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/mixed/rmp/canio.h 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,3 +1,23 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 John Sweeney & Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + #ifndef _CANIO_H_ #define _CANIO_H_ Modified: code/player/trunk/server/drivers/mixed/rmp/canio_kvaser.cc =================================================================== --- code/player/trunk/server/drivers/mixed/rmp/canio_kvaser.cc 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/mixed/rmp/canio_kvaser.cc 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,3 +1,22 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 John Sweeney & Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ #include "canio_kvaser.h" Modified: code/player/trunk/server/drivers/mixed/rmp/canio_kvaser.h =================================================================== --- code/player/trunk/server/drivers/mixed/rmp/canio_kvaser.h 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/mixed/rmp/canio_kvaser.h 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,3 +1,23 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 John Sweeney & Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + #ifndef _KVASER_CANLIB_ #define _KVASER_CANLIB_ Modified: code/player/trunk/server/drivers/planner/wavefront/plan.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan.c 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/planner/wavefront/plan.c 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,4 +1,26 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Andrew Howard + * Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /************************************************************************** * Desc: Path planning * Author: Andrew Howard Modified: code/player/trunk/server/drivers/planner/wavefront/plan.h =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan.h 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/planner/wavefront/plan.h 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,4 +1,26 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Andrew Howard + * Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /************************************************************************** * Desc: Path planning * Author: Andrew Howard Modified: code/player/trunk/server/drivers/planner/wavefront/plan_control.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan_control.c 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/planner/wavefront/plan_control.c 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,3 +1,25 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Andrew Howard + * Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + #include <stdlib.h> #include <stdio.h> #include <math.h> Modified: code/player/trunk/server/drivers/planner/wavefront/plan_plan.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan_plan.c 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/planner/wavefront/plan_plan.c 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,3 +1,25 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Andrew Howard + * Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /************************************************************************** * Desc: Path planner: plan generation * Author: Andrew Howard Modified: code/player/trunk/server/drivers/planner/wavefront/plan_waypoint.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan_waypoint.c 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/planner/wavefront/plan_waypoint.c 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,4 +1,26 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Andrew Howard + * Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /************************************************************************** * Desc: Path planner: waypoint generation * Author: Andrew Howard Modified: code/player/trunk/server/drivers/planner/wavefront/test.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/test.c 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/planner/wavefront/test.c 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,3 +1,25 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Andrew Howard + * Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + #include <stdio.h> #include <assert.h> #include <stdlib.h> Modified: code/player/trunk/server/drivers/position/vfh/vfh.cc =================================================================== --- code/player/trunk/server/drivers/position/vfh/vfh.cc 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/position/vfh/vfh.cc 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,3 +1,25 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2003 + * Chris Jones, Brian Gerkey, Alex Brooks, Richard Vaughan + * + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + #include <assert.h> #include <math.h> #include <fstream> Modified: code/player/trunk/server/drivers/shell/readlog_time.cc =================================================================== --- code/player/trunk/server/drivers/shell/readlog_time.cc 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/shell/readlog_time.cc 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,5 +1,6 @@ /* * Player - One Hell of a Robot Server + * Copyright (C) 2000 * Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard * * This program is free software; you can redistribute it and/or modify Modified: code/player/trunk/server/drivers/shell/readlog_time.h =================================================================== --- code/player/trunk/server/drivers/shell/readlog_time.h 2009-07-23 22:17:18 UTC (rev 8107) +++ code/player/trunk/server/drivers/shell/readlog_time.h 2009-07-23 23:03:37 UTC (rev 8108) @@ -1,5 +1,6 @@ /* * Player - One Hell of a Robot Server + * Copyright (C) 2000 * Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard * * This program is free software; you can redistribute it and/or modify This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2009-07-24 18:40:03
|
Revision: 8129 http://playerstage.svn.sourceforge.net/playerstage/?rev=8129&view=rev Author: thjc Date: 2009-07-24 18:39:50 +0000 (Fri, 24 Jul 2009) Log Message: ----------- more license clarification Modified Paths: -------------- code/player/trunk/server/drivers/laser/RS4Leuze_laser.cc code/player/trunk/server/drivers/laser/RS4Leuze_laser.h code/player/trunk/server/drivers/rfid/rfi341_protocol.cc code/player/trunk/server/drivers/rfid/rfi341_protocol.h Modified: code/player/trunk/server/drivers/laser/RS4Leuze_laser.cc =================================================================== --- code/player/trunk/server/drivers/laser/RS4Leuze_laser.cc 2009-07-24 17:19:09 UTC (rev 8128) +++ code/player/trunk/server/drivers/laser/RS4Leuze_laser.cc 2009-07-24 18:39:50 UTC (rev 8129) @@ -1,3 +1,23 @@ + /* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey et al. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /** laser.cpp V 2.0 -> RS4Leuze_laser.cpp Modified: code/player/trunk/server/drivers/laser/RS4Leuze_laser.h =================================================================== --- code/player/trunk/server/drivers/laser/RS4Leuze_laser.h 2009-07-24 17:19:09 UTC (rev 8128) +++ code/player/trunk/server/drivers/laser/RS4Leuze_laser.h 2009-07-24 18:39:50 UTC (rev 8129) @@ -1,3 +1,23 @@ + /* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey et al. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /** laser.h V 2.0 -> RS4Leuze_laser.cpp Modified: code/player/trunk/server/drivers/rfid/rfi341_protocol.cc =================================================================== --- code/player/trunk/server/drivers/rfid/rfi341_protocol.cc 2009-07-24 17:19:09 UTC (rev 8128) +++ code/player/trunk/server/drivers/rfid/rfi341_protocol.cc 2009-07-24 18:39:50 UTC (rev 8129) @@ -1,4 +1,25 @@ /* + * Player - One Hell of a Robot Server + * Copyright (C) 2007 + * Nico Blodow and Radu Bogdan Rusu + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +/* Desc: Driver for the SICK RFI341 unit Author: Nico Blodow and Radu Bogdan Rusu Date: 9 Mar 2007 Modified: code/player/trunk/server/drivers/rfid/rfi341_protocol.h =================================================================== --- code/player/trunk/server/drivers/rfid/rfi341_protocol.h 2009-07-24 17:19:09 UTC (rev 8128) +++ code/player/trunk/server/drivers/rfid/rfi341_protocol.h 2009-07-24 18:39:50 UTC (rev 8129) @@ -1,4 +1,25 @@ /* + * Player - One Hell of a Robot Server + * Copyright (C) 2007 + * Nico Blodow and Radu Bogdan Rusu + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +/* Desc: Driver for the SICK RFI341 unit Author: Nico Blodow and Radu Bogdan Rusu Date: 9 Mar 2007 This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <gb...@us...> - 2009-08-04 03:06:13
|
Revision: 8171 http://playerstage.svn.sourceforge.net/playerstage/?rev=8171&view=rev Author: gbiggs Date: 2009-08-04 03:06:07 +0000 (Tue, 04 Aug 2009) Log Message: ----------- Fixed bug #2825091 Modified Paths: -------------- code/player/trunk/server/drivers/mixed/phidgetIFK/phidgetIFK.cc code/player/trunk/server/drivers/wsn/phidgetAcc.cc Modified: code/player/trunk/server/drivers/mixed/phidgetIFK/phidgetIFK.cc =================================================================== --- code/player/trunk/server/drivers/mixed/phidgetIFK/phidgetIFK.cc 2009-08-04 03:00:26 UTC (rev 8170) +++ code/player/trunk/server/drivers/mixed/phidgetIFK/phidgetIFK.cc 2009-08-04 03:06:07 UTC (rev 8171) @@ -121,7 +121,6 @@ // Destructor ~PhidgetIFK(); - Shutdown // Must implement the following methods. virtual int MainSetup(); virtual void MainQuit(); @@ -561,13 +560,13 @@ gettimeofday( &tv_framerate_start, NULL ); if (done != 0) { - cout << "Error in nanosleep! ERRNO: " << errno << " "; + std::cout << "Error in nanosleep! ERRNO: " << errno << " "; if (errno == EINTR) { - cout << "EINTR" ; + std::cout << "EINTR" ; } else if (errno == EINVAL) { - cout << "EINVAL" ; + std::cout << "EINVAL" ; } - cout << endl; + std::cout << endl; } Modified: code/player/trunk/server/drivers/wsn/phidgetAcc.cc =================================================================== --- code/player/trunk/server/drivers/wsn/phidgetAcc.cc 2009-08-04 03:00:26 UTC (rev 8170) +++ code/player/trunk/server/drivers/wsn/phidgetAcc.cc 2009-08-04 03:06:07 UTC (rev 8171) @@ -109,7 +109,7 @@ ~PhidgetAcc(); virtual int MainSetup(); - virtual int MainQuit(); + virtual void MainQuit(); virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr * hdr, void * data); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <gb...@us...> - 2009-08-26 08:36:31
|
Revision: 8230 http://playerstage.svn.sourceforge.net/playerstage/?rev=8230&view=rev Author: gbiggs Date: 2009-08-26 08:36:23 +0000 (Wed, 26 Aug 2009) Log Message: ----------- Applied patch #2842767: some win32 issues Modified Paths: -------------- code/player/trunk/server/drivers/blobfinder/shapetracker/orientation.h code/player/trunk/server/drivers/blobfinder/shapetracker/shapetracker.cc code/player/trunk/server/drivers/blobfinder/simpleshape/simpleshape.cc code/player/trunk/server/drivers/blobfinder/upcbarcode/upcbarcode.cc code/player/trunk/server/drivers/camera/compress/cameracompress.cc code/player/trunk/server/drivers/camera/compress/camerauncompress.cc code/player/trunk/server/drivers/camera/cvcam/cvcam.cc code/player/trunk/server/drivers/camera/imageseq/imageseq.cc Modified: code/player/trunk/server/drivers/blobfinder/shapetracker/orientation.h =================================================================== --- code/player/trunk/server/drivers/blobfinder/shapetracker/orientation.h 2009-08-26 08:30:24 UTC (rev 8229) +++ code/player/trunk/server/drivers/blobfinder/shapetracker/orientation.h 2009-08-26 08:36:23 UTC (rev 8230) @@ -38,7 +38,7 @@ #include <stdio.h> #include <math.h> -#include <opencv/cv.h> +#include <cv.h> //Estimate the length between two points (cvpoint) Modified: code/player/trunk/server/drivers/blobfinder/shapetracker/shapetracker.cc =================================================================== --- code/player/trunk/server/drivers/blobfinder/shapetracker/shapetracker.cc 2009-08-26 08:30:24 UTC (rev 8229) +++ code/player/trunk/server/drivers/blobfinder/shapetracker/shapetracker.cc 2009-08-26 08:36:23 UTC (rev 8230) @@ -50,8 +50,8 @@ #include <libplayercore/playercore.h> #include <base/imagebase.h> -#include <opencv/cv.h> -//#include <opencv/highgui.h> +#include <cv.h> +//#include <highgui.h> #define winName "ShapeTracker" Modified: code/player/trunk/server/drivers/blobfinder/simpleshape/simpleshape.cc =================================================================== --- code/player/trunk/server/drivers/blobfinder/simpleshape/simpleshape.cc 2009-08-26 08:30:24 UTC (rev 8229) +++ code/player/trunk/server/drivers/blobfinder/simpleshape/simpleshape.cc 2009-08-26 08:36:23 UTC (rev 8230) @@ -106,8 +106,8 @@ #include <string.h> #include "../../base/imagebase.h" -#include <opencv/cv.h> -#include <opencv/highgui.h> +#include <cv.h> +#include <highgui.h> // Invariant feature set for a contour class FeatureSet Modified: code/player/trunk/server/drivers/blobfinder/upcbarcode/upcbarcode.cc =================================================================== --- code/player/trunk/server/drivers/blobfinder/upcbarcode/upcbarcode.cc 2009-08-26 08:30:24 UTC (rev 8229) +++ code/player/trunk/server/drivers/blobfinder/upcbarcode/upcbarcode.cc 2009-08-26 08:36:23 UTC (rev 8230) @@ -108,8 +108,8 @@ #include "../../base/imagebase.h" -#include <opencv/cv.h> -#include <opencv/highgui.h> +#include <cv.h> +#include <highgui.h> // Info on potential blobs. struct blob_t Modified: code/player/trunk/server/drivers/camera/compress/cameracompress.cc =================================================================== --- code/player/trunk/server/drivers/camera/compress/cameracompress.cc 2009-08-26 08:30:24 UTC (rev 8229) +++ code/player/trunk/server/drivers/camera/compress/cameracompress.cc 2009-08-26 08:36:23 UTC (rev 8230) @@ -75,10 +75,11 @@ #include <string.h> #include <stdlib.h> +#ifndef WIN32 #include <unistd.h> +#endif #include <stddef.h> #include <stdlib.h> // for atoi(3) -#include <netinet/in.h> // for htons(3) #include <math.h> #include <libplayercore/playercore.h> @@ -306,7 +307,11 @@ if (this->save) { +#ifdef WIN32 + _snprintf(filename, sizeof(filename), "click-%04d.jpeg",this->frameno++); +#else snprintf(filename, sizeof(filename), "click-%04d.jpeg",this->frameno++); +#endif FILE *fp = fopen(filename, "w+"); int ret = fwrite(this->data.image, 1, this->data.image_count, fp); if (ret < 0) Modified: code/player/trunk/server/drivers/camera/compress/camerauncompress.cc =================================================================== --- code/player/trunk/server/drivers/camera/compress/camerauncompress.cc 2009-08-26 08:30:24 UTC (rev 8229) +++ code/player/trunk/server/drivers/camera/compress/camerauncompress.cc 2009-08-26 08:36:23 UTC (rev 8230) @@ -77,9 +77,10 @@ #include <string.h> #include <stdlib.h> +#ifndef WIN32 #include <unistd.h> +#endif #include <stdlib.h> // for atoi(3) -#include <netinet/in.h> // for htons(3) #include <math.h> #include <libplayercore/playercore.h> @@ -247,7 +248,11 @@ if (this->save) { +#ifdef WIN32 + _snprintf(filename, sizeof(filename), "click-%04d.ppm",this->frameno++); +#else snprintf(filename, sizeof(filename), "click-%04d.ppm",this->frameno++); +#endif FILE *fp = fopen(filename, "w+"); int ret = fwrite (this->data.image, 1, this->data.image_count, fp); if (ret < 0) Modified: code/player/trunk/server/drivers/camera/cvcam/cvcam.cc =================================================================== --- code/player/trunk/server/drivers/camera/cvcam/cvcam.cc 2009-08-26 08:30:24 UTC (rev 8229) +++ code/player/trunk/server/drivers/camera/cvcam/cvcam.cc 2009-08-26 08:36:23 UTC (rev 8230) @@ -87,8 +87,8 @@ #include <pthread.h> #include <libplayercore/playercore.h> -#include <opencv/cv.h> -#include <opencv/highgui.h> +#include <cv.h> +#include <highgui.h> //--------------------------------- Modified: code/player/trunk/server/drivers/camera/imageseq/imageseq.cc =================================================================== --- code/player/trunk/server/drivers/camera/imageseq/imageseq.cc 2009-08-26 08:30:24 UTC (rev 8229) +++ code/player/trunk/server/drivers/camera/imageseq/imageseq.cc 2009-08-26 08:36:23 UTC (rev 8230) @@ -87,17 +87,19 @@ #include <string.h> #include <stdlib.h> +#include <stddef.h> +#ifndef WIN32 #include <unistd.h> +#endif +#include <time.h> // for nanosleep() and struct timespec #include <stdlib.h> // for atoi(3) -#include <netinet/in.h> // for htons(3) #include <math.h> -#include <opencv/cv.h> -#include <opencv/highgui.h> +#include <cv.h> +#include <highgui.h> #include <libplayercore/playercore.h> - class ImageSeq : public ThreadedDriver { // Constructor @@ -175,7 +177,11 @@ pthread_testcancel(); // Compose filename +#ifdef WIN32 + _snprintf(filename, sizeof(filename), this->pattern, this->frame); +#else snprintf(filename, sizeof(filename), this->pattern, this->frame); +#endif // Load the image if (this->LoadImage(filename) != 0) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |