From: <gb...@us...> - 2008-07-16 17:01:23
|
Revision: 6877 http://playerstage.svn.sourceforge.net/playerstage/?rev=6877&view=rev Author: gbiggs Date: 2008-07-17 00:01:32 +0000 (Thu, 17 Jul 2008) Log Message: ----------- Tidied up cmake output formatting a little to make things clearer. Modified Paths: -------------- code/player/trunk/client_libs/CMakeLists.txt code/player/trunk/cmake/internal/DriverUtils.cmake code/player/trunk/examples/CMakeLists.txt code/player/trunk/utils/CMakeLists.txt Modified: code/player/trunk/client_libs/CMakeLists.txt =================================================================== --- code/player/trunk/client_libs/CMakeLists.txt 2008-07-16 23:34:49 UTC (rev 6876) +++ code/player/trunk/client_libs/CMakeLists.txt 2008-07-17 00:01:32 UTC (rev 6877) @@ -1,3 +1,4 @@ MESSAGE (STATUS "===== Client Libraries =====") ADD_SUBDIRECTORY (libplayerc) -ADD_SUBDIRECTORY (libplayerc++) \ No newline at end of file +ADD_SUBDIRECTORY (libplayerc++) +MESSAGE (STATUS "============================") Modified: code/player/trunk/cmake/internal/DriverUtils.cmake =================================================================== --- code/player/trunk/cmake/internal/DriverUtils.cmake 2008-07-16 23:34:49 UTC (rev 6876) +++ code/player/trunk/cmake/internal/DriverUtils.cmake 2008-07-17 00:01:32 UTC (rev 6877) @@ -448,9 +448,9 @@ GET_FROM_GLOBAL_MAP (_reason PLAYER_NOT_BUILT_REASONS ${_driverName}) MESSAGE (STATUS "${_driverName} - ${_reason}") ENDFOREACH (_driverName) - MESSAGE (STATUS "") IF (PLAYERDRIVER_DEFINES) + MESSAGE (STATUS "") MESSAGE (STATUS "Driver defines:") FOREACH (_define ${PLAYERDRIVER_DEFINES}) IF (${_define}) @@ -460,8 +460,9 @@ ENDIF (${_define}) MESSAGE (STATUS "${_define} - ${_value}") ENDFOREACH (_define ${PLAYERDRIVER_DEFINES}) - MESSAGE (STATUS "") ENDIF (PLAYERDRIVER_DEFINES) + MESSAGE (STATUS "===================") + MESSAGE (STATUS "") ENDMACRO (WRITE_DRIVER_REPORT) ############################################################################### Modified: code/player/trunk/examples/CMakeLists.txt =================================================================== --- code/player/trunk/examples/CMakeLists.txt 2008-07-16 23:34:49 UTC (rev 6876) +++ code/player/trunk/examples/CMakeLists.txt 2008-07-17 00:01:32 UTC (rev 6877) @@ -6,4 +6,5 @@ ADD_SUBDIRECTORY (libplayerc) ADD_SUBDIRECTORY (libplayerc++) ENDIF (BUILD_EXAMPLES) +MESSAGE (STATUS "====================") MESSAGE (STATUS "") Modified: code/player/trunk/utils/CMakeLists.txt =================================================================== --- code/player/trunk/utils/CMakeLists.txt 2008-07-16 23:34:49 UTC (rev 6876) +++ code/player/trunk/utils/CMakeLists.txt 2008-07-17 00:01:32 UTC (rev 6877) @@ -16,3 +16,4 @@ ELSE (BUILD_UTILS) MESSAGE (STATUS "Disabled by user. Use ccmake to enable.") ENDIF (BUILD_UTILS) +MESSAGE (STATUS "============================") This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <gb...@us...> - 2008-07-16 18:31:29
|
Revision: 6878 http://playerstage.svn.sourceforge.net/playerstage/?rev=6878&view=rev Author: gbiggs Date: 2008-07-17 01:31:38 +0000 (Thu, 17 Jul 2008) Log Message: ----------- Fixed up finding libraries in non-standard locations Modified Paths: -------------- code/player/trunk/CMakeLists.txt code/player/trunk/cmake/internal/SearchForStuff.cmake Modified: code/player/trunk/CMakeLists.txt =================================================================== --- code/player/trunk/CMakeLists.txt 2008-07-17 00:01:32 UTC (rev 6877) +++ code/player/trunk/CMakeLists.txt 2008-07-17 01:31:38 UTC (rev 6878) @@ -47,6 +47,9 @@ # Set some common include directories, including the binary dir to get config.h INCLUDE_DIRECTORIES (${PROJECT_SOURCE_DIR} ${PROJECT_BINARY_DIR} ${PROJECT_BINARY_DIR}/libplayercore) +# Extra directories where needed libraries, etc may be located (set by the user) +INCLUDE_DIRECTORIES (${PLAYER_EXTRA_INCLUDE_DIRS}) +LINK_DIRECTORIES (${PLAYER_EXTRA_LIB_DIRS}) # If on Windows, only build the client libraries. (this won't actually work yet cause libplayerc needs some stuff from core) ADD_SUBDIRECTORY (client_libs) Modified: code/player/trunk/cmake/internal/SearchForStuff.cmake =================================================================== --- code/player/trunk/cmake/internal/SearchForStuff.cmake 2008-07-17 00:01:32 UTC (rev 6877) +++ code/player/trunk/cmake/internal/SearchForStuff.cmake 2008-07-17 01:31:38 UTC (rev 6878) @@ -4,6 +4,12 @@ INCLUDE (CheckIncludeFiles) INCLUDE (CheckLibraryExists) +SET (PLAYER_EXTRA_LIB_DIRS "" CACHE STRING "List of extra library directories.") +MARK_AS_ADVANCED (PLAYER_EXTRA_LIB_DIRS) +SET (PLAYER_EXTRA_INCLUDE_DIRS "" CACHE STRING "List of extra include directories.") +MARK_AS_ADVANCED (PLAYER_EXTRA_INCLUDE_DIRS) +SET (CMAKE_REQUIRED_INCLUDES ${PLAYER_EXTRA_INCLUDE_DIRS}) + IF (PLAYER_OS_QNX) SET (CMAKE_REQUIRED_LIBRARIES socket) ELSEIF (PLAYER_OS_SOLARIS) @@ -32,24 +38,17 @@ CHECK_INCLUDE_FILES (strings.h HAVE_STRINGS_H) CHECK_INCLUDE_FILES (dns_sd.h HAVE_DNS_SD) IF (HAVE_DNS_SD) - SET (DNS_SD_LOCATION "" CACHE STRING "Location where dns_sd is installed.") - MARK_AS_ADVANCED (DNS_SD_LOCATION) - CHECK_LIBRARY_EXISTS (dns_sd DNSServiceRefDeallocate "${DNS_SD_LOCATION}" HAVE_DNS_SD) + CHECK_LIBRARY_EXISTS (dns_sd DNSServiceRefDeallocate "${PLAYER_EXTRA_LIB_DIRS}" HAVE_DNS_SD) ENDIF (HAVE_DNS_SD) +CHECK_LIBRARY_EXISTS (ltdl lt_dlopenext "${PLAYER_EXTRA_LIB_DIRS}" HAVE_LIBLTDL) -SET (LIBLTDL_LOCATION "" CACHE STRING "Location where libltdl is installed.") -MARK_AS_ADVANCED (LIBLTDL_LOCATION) -CHECK_LIBRARY_EXISTS (ltdl lt_dlopenext "${LIBLTDL_LOCATION}" HAVE_LIBLTDL) - SET (CMAKE_REQUIRED_INCLUDES math.h) SET (CMAKE_REQUIRED_LIBRARIES m) CHECK_FUNCTION_EXISTS (round HAVE_ROUND) SET (CMAKE_REQUIRED_INCLUDES) SET (CMAKE_REQUIRED_LIBRARIES) -SET (LIBJPEG_LOCATION "" CACHE STRING "Location where libjpeg is installed.") -MARK_AS_ADVANCED (LIBJPEG_LOCATION) -CHECK_LIBRARY_EXISTS (jpeg jpeg_read_header "${LIBJPEG_LOCATION}" HAVE_LIBJPEG) +CHECK_LIBRARY_EXISTS (jpeg jpeg_read_header "${PLAYER_EXTRA_LIB_DIRS}" HAVE_LIBJPEG) CHECK_INCLUDE_FILES ("stdio.h;jpeglib.h" HAVE_JPEGLIB_H) IF (HAVE_LIBJPEG AND HAVE_JPEGLIB_H) SET (HAVE_JPEG TRUE) @@ -61,17 +60,13 @@ SET (CMAKE_REQUIRED_INCLUDES) SET (CMAKE_REQUIRED_LIBRARIES) -SET (LIBZ_LOCATION "" CACHE STRING "Location where libz is installed.") -MARK_AS_ADVANCED (LIBZ_LOCATION) -CHECK_LIBRARY_EXISTS (z compress2 "${LIBZ_LOCATION}" HAVE_LIBZ) +CHECK_LIBRARY_EXISTS (z compress2 "${PLAYER_EXTRA_LIB_DIRS}" HAVE_LIBZ) CHECK_INCLUDE_FILES (zlib.h HAVE_ZLIB_H) IF (HAVE_LIBZ AND HAVE_ZLIB_H) SET (HAVE_Z TRUE) ENDIF (HAVE_LIBZ AND HAVE_ZLIB_H) -SET (LIBRT_LOCATION "" CACHE STRING "Location where librt is installed.") -MARK_AS_ADVANCED (LIBRT_LOCATION) -CHECK_LIBRARY_EXISTS (rt clock_gettime "${LIBRT_LOCATION}" HAVE_LIBRT) +CHECK_LIBRARY_EXISTS (rt clock_gettime "${PLAYER_EXTRA_LIB_DIRS}" HAVE_LIBRT) SET (CMAKE_REQUIRED_LIBRARIES rt) CHECK_FUNCTION_EXISTS (clock_gettime HAVE_CLOCK_GETTIME_FUNC) SET (CMAKE_REQUIRED_LIBRARIES) @@ -134,3 +129,5 @@ FILE (WRITE ${testSTLSource} "#include <string>\nint main () {std::string a = \"blag\"; return 0;}\n") TRY_COMPILE (HAVE_STL ${CMAKE_CURRENT_BINARY_DIR} ${testSTLSource}) + +SET (CMAKE_REQUIRED_INCLUDES) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2008-07-22 23:01:10
|
Revision: 6908 http://playerstage.svn.sourceforge.net/playerstage/?rev=6908&view=rev Author: thjc Date: 2008-07-23 06:01:19 +0000 (Wed, 23 Jul 2008) Log Message: ----------- Merged 6672 from 2-1 added so extra tests to prevent deadlocks in drivers that make player requests in their shutdown methods. These could deadlock while the server was shutting down. Modified Paths: -------------- code/player/trunk/libplayercore/device.cc code/player/trunk/libplayercore/devicetable.cc Property Changed: ---------------- code/player/trunk/ Property changes on: code/player/trunk ___________________________________________________________________ Added: svn:mergeinfo + /code/player/branches/release-2-1-patches:6672 Modified: code/player/trunk/libplayercore/device.cc =================================================================== --- code/player/trunk/libplayercore/device.cc 2008-07-23 05:30:31 UTC (rev 6907) +++ code/player/trunk/libplayercore/device.cc 2008-07-23 06:01:19 UTC (rev 6908) @@ -258,6 +258,9 @@ double* timestamp, bool threaded) { + // check driver still ahs subscriptions, stops deadlocks on server shutdown + if (driver->subscriptions == 0) + return NULL; // Send the request message this->PutMsg(resp_queue, type, subtype, @@ -282,13 +285,13 @@ // non-threaded, then his ProcessMessage() would get called // recursively). - Message* msg; + Message* msg = NULL; if(threaded) { - resp_queue->Wait(); // HACK: this loop should not be neccesary! // pthread_cond_wait does not garuntee no false wake up, so maybe it is. - while(!(msg = resp_queue->Pop())) + // test driver is still subscribed to prevent deadlocks on server shutdown + while(driver->subscriptions > 0 && !(msg = resp_queue->Pop())) { PLAYER_WARN("empty queue after waiting!"); resp_queue->Wait(); // this is a cancelation point Modified: code/player/trunk/libplayercore/devicetable.cc =================================================================== --- code/player/trunk/libplayercore/devicetable.cc 2008-07-23 05:30:31 UTC (rev 6907) +++ code/player/trunk/libplayercore/devicetable.cc 2008-07-23 06:01:19 UTC (rev 6908) @@ -74,6 +74,8 @@ thisentry->driver->Shutdown(); thisentry->driver->subscriptions = 0; thisentry->driver->alwayson = 0; + // wake up anything waiting on the drivers queue, so they can notice it is dead + thisentry->driver->InQueue->DataAvailable(); } thisentry = thisentry->next; } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2008-07-22 23:07:55
|
Revision: 6909 http://playerstage.svn.sourceforge.net/playerstage/?rev=6909&view=rev Author: thjc Date: 2008-07-23 06:08:05 +0000 (Wed, 23 Jul 2008) Log Message: ----------- Merged 6673 from 2-1 fixed uninitialised memory in array reallocation. Modified Paths: -------------- code/player/trunk/libplayercore/filewatcher.cc Property Changed: ---------------- code/player/trunk/ Property changes on: code/player/trunk ___________________________________________________________________ Modified: svn:mergeinfo - /code/player/branches/release-2-1-patches:6672 + /code/player/branches/release-2-1-patches:6672-6673 Modified: code/player/trunk/libplayercore/filewatcher.cc =================================================================== --- code/player/trunk/libplayercore/filewatcher.cc 2008-07-23 06:01:19 UTC (rev 6908) +++ code/player/trunk/libplayercore/filewatcher.cc 2008-07-23 06:08:05 UTC (rev 6909) @@ -145,8 +145,10 @@ if (next_entry == NULL) { // otherwise we allocate some more room for the array + size_t orig_size = WatchedFilesArraySize; WatchedFilesArraySize*=2; WatchedFiles = reinterpret_cast<struct fd_driver_pair *> (realloc(WatchedFiles,sizeof(WatchedFiles[0])*WatchedFilesArraySize)); + memset(&WatchedFiles[orig_size],0,sizeof(WatchedFiles[0])*(WatchedFilesArraySize-orig_size)); next_entry = &WatchedFiles[WatchedFilesArrayCount]; } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2008-07-22 23:22:49
|
Revision: 6910 http://playerstage.svn.sourceforge.net/playerstage/?rev=6910&view=rev Author: thjc Date: 2008-07-23 06:22:57 +0000 (Wed, 23 Jul 2008) Log Message: ----------- Merged 6738 from 2-1 added missing headers that caused build issues on gcc 4.3 Modified Paths: -------------- code/player/trunk/server/drivers/laser/lms400_cola.cc code/player/trunk/server/drivers/mixed/mricp/src/geometry2D.cpp code/player/trunk/server/drivers/mixed/mricp/src/map.cpp Property Changed: ---------------- code/player/trunk/ Property changes on: code/player/trunk ___________________________________________________________________ Modified: svn:mergeinfo - /code/player/branches/release-2-1-patches:6672-6673 + /code/player/branches/release-2-1-patches:6672-6673,6738 Modified: code/player/trunk/server/drivers/laser/lms400_cola.cc =================================================================== --- code/player/trunk/server/drivers/laser/lms400_cola.cc 2008-07-23 06:08:05 UTC (rev 6909) +++ code/player/trunk/server/drivers/laser/lms400_cola.cc 2008-07-23 06:22:57 UTC (rev 6910) @@ -9,6 +9,7 @@ #include <libplayercore/playercore.h> #include "lms400_cola.h" +#include <unistd.h> //////////////////////////////////////////////////////////////////////////////// // Constructor. Modified: code/player/trunk/server/drivers/mixed/mricp/src/geometry2D.cpp =================================================================== --- code/player/trunk/server/drivers/mixed/mricp/src/geometry2D.cpp 2008-07-23 06:08:05 UTC (rev 6909) +++ code/player/trunk/server/drivers/mixed/mricp/src/geometry2D.cpp 2008-07-23 06:22:57 UTC (rev 6910) @@ -5,6 +5,7 @@ #include "geometry2D.h" #include <cmath> #include <cassert> +#include <stdio.h> using namespace std; Modified: code/player/trunk/server/drivers/mixed/mricp/src/map.cpp =================================================================== --- code/player/trunk/server/drivers/mixed/mricp/src/map.cpp 2008-07-23 06:08:05 UTC (rev 6909) +++ code/player/trunk/server/drivers/mixed/mricp/src/map.cpp 2008-07-23 06:22:57 UTC (rev 6910) @@ -19,6 +19,7 @@ ***************************************************************************/ #include "map.h" #include <math.h> +#include <stdlib.h> #include <cassert> #include <iostream> using namespace std; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2008-07-22 23:34:21
|
Revision: 6911 http://playerstage.svn.sourceforge.net/playerstage/?rev=6911&view=rev Author: thjc Date: 2008-07-23 06:34:31 +0000 (Wed, 23 Jul 2008) Log Message: ----------- Merged 6834 from 2-1 Added scan point calculation to the ranger device Modified Paths: -------------- code/player/trunk/client_libs/libplayerc/dev_ranger.c code/player/trunk/client_libs/libplayerc/playerc.h Property Changed: ---------------- code/player/trunk/ Property changes on: code/player/trunk ___________________________________________________________________ Modified: svn:mergeinfo - /code/player/branches/release-2-1-patches:6672-6673,6738 + /code/player/branches/release-2-1-patches:6672-6673,6738,6834 Modified: code/player/trunk/client_libs/libplayerc/dev_ranger.c =================================================================== --- code/player/trunk/client_libs/libplayerc/dev_ranger.c 2008-07-23 06:22:57 UTC (rev 6910) +++ code/player/trunk/client_libs/libplayerc/dev_ranger.c 2008-07-23 06:34:31 UTC (rev 6911) @@ -38,6 +38,7 @@ */ #include <string.h> +#include <math.h> #include "playerc.h" #include "error.h" @@ -72,6 +73,10 @@ free(device->sensor_poses); if(device->sensor_sizes != NULL) free(device->sensor_sizes); + if(device->bearings != NULL) + free(device->bearings); + if(device->points != NULL) + free(device->points); free(device); } @@ -90,16 +95,74 @@ } +// Calculate bearings +void playerc_ranger_calculate_bearings(playerc_ranger_t *device) +{ + device->bearings_count = device->ranges_count; + if (device->bearings_count == 0 && device->bearings != NULL) + { + free(device->bearings); + device->bearings = NULL; + } + else + { + if((device->bearings = (double *) realloc(device->bearings, device->bearings_count * sizeof(double))) == NULL) + { + device->bearings_count = 0; + PLAYER_ERROR("Failed to allocate space to store bearings"); + return; + } + + double b = device->min_angle; + uint32_t ii; + for (ii = 0; ii < device->bearings_count; ii++) + { + device->bearings[ii] = b; + b += device->resolution; + } + } +} + + +// Calculate scan points +void playerc_ranger_calculate_points(playerc_ranger_t *device) +{ + device->points_count = device->ranges_count; + if (device->points_count == 0 && device->points != NULL) + { + free(device->points); + device->points = NULL; + } + else + { + if((device->points = (player_point_3d_t *) realloc(device->points, device->points_count * sizeof(player_point_3d_t))) == NULL) + { + device->points_count = 0; + PLAYER_ERROR("Failed to allocate space to store points"); + return; + } + + double b = device->min_angle; + uint32_t ii; + for (ii = 0; ii < device->points_count; ii++) + { + double r = device->ranges[ii]; + device->points[ii].px = r * cos(b); + device->points[ii].py = r * sin(b); + device->points[ii].pz = 0.0; + b += device->resolution; + } + } +} + + // Copy range data to the device void playerc_ranger_copy_range_data(playerc_ranger_t *device, player_ranger_data_range_t *data) { if (device->ranges_count != data->ranges_count || device->ranges == NULL) { - // The number of data has changed, so delete any old data - if(device->ranges != NULL) - free(device->ranges); // Allocate memory for the new data - if((device->ranges = (double *) malloc(data->ranges_count * sizeof(double))) == NULL) + if((device->ranges = (double *) realloc(device->ranges, data->ranges_count * sizeof(double))) == NULL) { device->ranges_count = 0; PLAYER_ERROR("Failed to allocate space to store range data"); @@ -117,11 +180,8 @@ { if (device->intensities_count != data->intensities_count || device->intensities == NULL) { - // The number of data has changed, so delete any old data - if(device->intensities != NULL) - free(device->intensities); // Allocate memory for the new data - if((device->intensities = (double *) malloc(data->intensities_count * sizeof(double))) == NULL) + if((device->intensities = (double *) realloc(device->intensities, data->intensities_count * sizeof(double))) == NULL) { device->intensities_count = 0; PLAYER_ERROR("Failed to allocate space to store intensity data"); @@ -189,11 +249,15 @@ if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_RANGER_DATA_RANGE)) { playerc_ranger_copy_range_data(device, (player_ranger_data_range_t *) data); + playerc_ranger_calculate_bearings(device); + playerc_ranger_calculate_points(device); } else if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_RANGER_DATA_RANGEPOSE)) { playerc_ranger_copy_range_data(device, &((player_ranger_data_rangepose_t *) data)->data); playerc_ranger_copy_geom(device, &((player_ranger_data_rangepose_t *) data)->geom); + playerc_ranger_calculate_bearings(device); + playerc_ranger_calculate_points(device); } else if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_RANGER_DATA_INTNS)) { Modified: code/player/trunk/client_libs/libplayerc/playerc.h =================================================================== --- code/player/trunk/client_libs/libplayerc/playerc.h 2008-07-23 06:22:57 UTC (rev 6910) +++ code/player/trunk/client_libs/libplayerc/playerc.h 2008-07-23 06:34:31 UTC (rev 6911) @@ -2979,6 +2979,16 @@ properties include intns_on for laser devices and volt_on for IR devices. */ double *intensities; + /** Number of individual scan bearings. */ + uint32_t bearings_count; + /** Scan bearings [radians]. */ + double *bearings; + + /** Number of scan points. */ + uint32_t points_count; + /** Scan points (x, y, z). */ + player_point_3d_t *points; + } playerc_ranger_t; /** @brief Create a ranger proxy. */ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2008-07-23 00:14:22
|
Revision: 6913 http://playerstage.svn.sourceforge.net/playerstage/?rev=6913&view=rev Author: thjc Date: 2008-07-23 07:14:31 +0000 (Wed, 23 Jul 2008) Log Message: ----------- Merged 6886 and 6907 from 2-1 blackboard interface/driver and proxy fixes Modified Paths: -------------- code/player/trunk/client_libs/libplayerc/bindings/python/playerc.i code/player/trunk/client_libs/libplayerc/dev_blackboard.c code/player/trunk/client_libs/libplayerc/playerc.h code/player/trunk/libplayercore/interfaces/064_blackboard.def code/player/trunk/server/drivers/blackboard/localbb/localbb.cpp Property Changed: ---------------- code/player/trunk/ Property changes on: code/player/trunk ___________________________________________________________________ Modified: svn:mergeinfo - /code/player/branches/release-2-1-patches:6672-6673,6738,6834 + /code/player/branches/release-2-1-patches:6672-6673,6738,6834,6886 Modified: code/player/trunk/client_libs/libplayerc/bindings/python/playerc.i =================================================================== --- code/player/trunk/client_libs/libplayerc/bindings/python/playerc.i 2008-07-23 06:40:18 UTC (rev 6912) +++ code/player/trunk/client_libs/libplayerc/bindings/python/playerc.i 2008-07-23 07:14:31 UTC (rev 6913) @@ -28,7 +28,7 @@ memset(buffer, 0, blength ); //result is of type playerc_rfidtag_t - unsigned int j; + unsigned int j; unsigned int guid_count=$1[i].guid_count; //copy the bytes into the buffer @@ -416,432 +416,672 @@ %extend playerc_blackboard { -static void python_on_blackboard_event(playerc_blackboard_t *device, player_blackboard_entry_t entry) + +#define DICT_GROUPS_INDEX 0 +#define DICT_SUBSCRIPTION_DATA_INDEX 1 +#define LIST_EVENTS_INDEX 2 +#define BOOL_QUEUE_EVENTS_INDEX 3 + +// Helper function to convert a c blackboard_entry_t into a python dictionary object. +// Returns a new reference to a dictionary object. +PyObject *__convert_blackboard_entry__(player_blackboard_entry_t *entry) { - char * str; - int i; - double d; - PyObject *data, *entry_dict, *group_dict, *groups, *list; - groups = (PyObject *)PyTuple_GetItem(device->py_private,0); - assert(groups); + PyObject *entry_dict, *data; + char* str; + int i; + double d; + int ok; - group_dict = (PyObject*) PyDict_GetItemString(groups, entry.group); - if (!group_dict) - { - return; - } + entry_dict = PyDict_New(); // New reference + assert (entry_dict); + + assert(entry); + assert(entry->key); + assert(entry->key_count > 0); + assert(entry->group); + assert(entry->group_count > 0); + + ok = PyDict_SetItemString(entry_dict, "key", PyString_FromString(entry->key)); // Steals reference + if (ok != 0) + { + PyErr_SetString(PyExc_RuntimeError, "Could not set dictionary value for 'key'"); + Py_XDECREF(entry_dict); + return NULL; + } + + ok = PyDict_SetItemString(entry_dict, "group", PyString_FromString(entry->group)); // Steals reference + if (ok != 0) + { + PyErr_SetString(PyExc_RuntimeError, "Could not set dictionary value for 'group'"); + Py_XDECREF(entry_dict); + return NULL; + } - // find our key in the group, if it exists - entry_dict = (PyObject*) PyDict_GetItemString(group_dict, entry.key); - if (!entry_dict) - { - return; - } + ok = PyDict_SetItemString(entry_dict, "type", PyLong_FromLong(entry->type)); // Steals reference + if (ok != 0) + { + PyErr_SetString(PyExc_RuntimeError, "Could not set dictionary value for 'type'"); + Py_XDECREF(entry_dict); + return NULL; + } - PyDict_SetItemString(entry_dict, "type", PyLong_FromLong(entry.type)); - PyDict_SetItemString(entry_dict, "subtype", PyLong_FromLong(entry.subtype)); - PyDict_SetItemString(entry_dict, "timestamp_sec", PyLong_FromLong(entry.timestamp_sec)); - PyDict_SetItemString(entry_dict, "timestamp_usec", PyLong_FromLong(entry.timestamp_usec)); - list = (PyObject*)PyTuple_GetItem(device->py_private,1); - assert(list); - data = NULL; - switch(entry.subtype) + ok = PyDict_SetItemString(entry_dict, "subtype", PyLong_FromLong(entry->subtype)); // Steals reference + if (ok != 0) + { + PyErr_SetString(PyExc_RuntimeError, "Could not set dictionary value for 'subtype'"); + Py_XDECREF(entry_dict); + return NULL; + } + + ok = PyDict_SetItemString(entry_dict, "timestamp_sec", PyLong_FromLong(entry->timestamp_sec)); // Steals reference + if (ok != 0) + { + PyErr_SetString(PyExc_RuntimeError, "Could not set dictionary value for 'timestamp_sec'"); + Py_XDECREF(entry_dict); + return NULL; + } + + ok = PyDict_SetItemString(entry_dict, "timestamp_usec", PyLong_FromLong(entry->timestamp_usec)); // Steals reference + if (ok != 0) + { + PyErr_SetString(PyExc_RuntimeError, "Could not set dictionary value for 'timestamp_usec'"); + Py_XDECREF(entry_dict); + return NULL; + } + + switch(entry->subtype) + { + case PLAYERC_BLACKBOARD_DATA_SUBTYPE_NONE: + data = Py_None; + Py_INCREF(data); + break; + case PLAYERC_BLACKBOARD_DATA_SUBTYPE_STRING: + assert(entry->type == PLAYERC_BLACKBOARD_DATA_TYPE_COMPLEX); + str = malloc(entry->data_count); + if (!str) + { + Py_XDECREF(entry_dict); + return PyErr_NoMemory(); + } + memcpy(str, entry->data, entry->data_count); + data = PyString_FromString(str); + break; + case PLAYERC_BLACKBOARD_DATA_SUBTYPE_INT: + assert(entry->type == PLAYERC_BLACKBOARD_DATA_TYPE_SIMPLE); + i = 0; + memcpy(&i, entry->data, entry->data_count); + data = PyLong_FromLong(i); + break; + case PLAYERC_BLACKBOARD_DATA_SUBTYPE_DOUBLE: + assert(entry->type == PLAYERC_BLACKBOARD_DATA_TYPE_SIMPLE); + d = 0.0; + memcpy(&d, entry->data, entry->data_count); + data = PyFloat_FromDouble(d); + break; + default: + data = Py_None; + Py_INCREF(data); + break; + } + + ok = PyDict_SetItemString(entry_dict, "data", data); // Steals reference + if (ok != 0) + { + PyErr_SetString(PyExc_RuntimeError, "Could not set dictionary value for 'data'"); + Py_XDECREF(entry_dict); + return NULL; + } + + return entry_dict; +} + +// Helper function which sets a value in a nested dictionary. Equivalent of dict['group']['key'] = value. +// Steals the entry reference. +PyObject *__set_nested_dictionary_entry__(PyObject *dict, const char* key, const char* group, PyObject *entry) +{ + PyObject *group_dict; + int ok; + int create_dict = 0; + + // Check we got valid arguments. + if (!PyDict_Check(dict)) + { + PyErr_SetString(PyExc_TypeError, "Expected type 'dict' for first argument (dict)"); + return NULL; + } + + if (!entry) + { + PyErr_SetString(PyExc_TypeError, "Expected type non NULL for fourth argument (entry)"); + return NULL; + } + + if (key == NULL || strlen(key) == 0) + { + PyErr_SetString(PyExc_TypeError, "Expected type 'string' for second argument (key)"); + return NULL; + } + + if (group == NULL || strlen(group) == 0) + { + PyErr_SetString(PyExc_TypeError, "Expected type 'string' for second argument (group)"); + return NULL; + } + + // Get the dictionary for the entry's group. If it doesn't exist, create it. + group_dict = (PyObject*)PyDict_GetItemString(dict, group); // Borrowed reference + if (!group_dict) + { + create_dict = 1; + group_dict = PyDict_New(); // New reference + } + + ok = PyDict_SetItemString(group_dict, key, entry); // Steals reference + if (ok != 0) + { + PyErr_SetString(PyExc_RuntimeError, "Failed to set dictionary entry"); + if (create_dict) { - case PLAYERC_BLACKBOARD_DATA_SUBTYPE_NONE: - data = Py_None; - break; - case PLAYERC_BLACKBOARD_DATA_SUBTYPE_STRING: - assert(entry.type == PLAYERC_BLACKBOARD_DATA_TYPE_COMPLEX); - str = malloc(entry.data_count); - assert(str); - memcpy(str, entry.data, entry.data_count); - data = PyString_FromString(str); - break; - case PLAYERC_BLACKBOARD_DATA_SUBTYPE_INT: - assert(entry.type == PLAYERC_BLACKBOARD_DATA_TYPE_SIMPLE); - i = 0; - memcpy(&i, entry.data, entry.data_count); - data = PyLong_FromLong(i); - break; - case PLAYERC_BLACKBOARD_DATA_SUBTYPE_DOUBLE: - assert(entry.type == PLAYERC_BLACKBOARD_DATA_TYPE_SIMPLE); - d = 0.0; - memcpy(&d, entry.data, entry.data_count); - data = PyFloat_FromDouble(d); - break; - default: - data = Py_None; - break; + Py_XDECREF(group_dict); } + return NULL; + } - assert(data != NULL); - PyDict_SetItemString(entry_dict, "data", data); - - if (PyLong_AsLong(PyTuple_GetItem(device->py_private,2)) > 0) - { - list = (PyObject*)PyTuple_GetItem(device->py_private,1); - assert(list); - PyList_Append(list, PyDict_Copy(entry_dict)); - } + // If we created the dictionary, we need to put it in the parent dictionary. + if (create_dict == 1) + { + ok = PyDict_SetItemString(dict, group, group_dict); // Steals reference + if (ok != 0) + { + PyErr_SetString(PyExc_RuntimeError, "Failed to set dictionary entry"); + Py_XDECREF(group_dict); + return NULL; + } + } + + Py_RETURN_NONE; } -// Will always return a list object. Returns a list of dictionary objects containing the event data. -PyObject *get_events() +// Helper function used to keep track to the number of subscriptions a group/key combination has +int __increment_reference_count__(PyObject *dict, const char* key, const char* group, int inc) { - PyObject *list, *copy; - int i, j; + PyObject *group_dict, *entry; + int old_value, new_value; + + // Get the dictionary for the entry's group. If it doesn't exist. + group_dict = (PyObject*)PyDict_GetItemString(dict, group); // Borrowed reference + if (!group_dict) + { + playerc_blackboard___set_nested_dictionary_entry__(self, dict, key, group, (PyObject*)PyInt_FromLong(inc)); // Steals reference to entry + return inc; + } + + entry = PyDict_GetItemString(group_dict, key); // Borrowed reference + if (entry) + { + old_value = PyLong_AsLong(entry); + } + else + { + old_value = 0; + } + + new_value = old_value + inc; + if (new_value < 0) + { + new_value = 0; + } + + playerc_blackboard___set_nested_dictionary_entry__(self, dict, key, group, PyInt_FromLong(new_value)); // Steals reference to entry + + return new_value; +} + +// Replacement for the existing on_blackboard_event method. The python implementation uses this instead. +static void __python_on_blackboard_event__(playerc_blackboard_t *device, player_blackboard_entry_t entry) +{ + PyObject *entry_dict, *groups_dict, *list, *queue_events; + + // Get the main groups dictionary + assert(device->py_private); + groups_dict = (PyObject*)PyTuple_GetItem(device->py_private, DICT_GROUPS_INDEX); // Borrowed reference + assert(groups_dict); + + // Get a dictionary of the entry + entry_dict = playerc_blackboard___convert_blackboard_entry__(device, &entry); // New reference + assert(entry_dict); + + // If we are queueing events, add them to the list + queue_events = PyTuple_GetItem(device->py_private, BOOL_QUEUE_EVENTS_INDEX); // Borrowed reference + assert(queue_events); + if (PyLong_AsLong(queue_events) != 0) + { + list = (PyObject*)PyTuple_GetItem(device->py_private, LIST_EVENTS_INDEX); // Borrowed reference + assert(list); + PyList_Append(list, entry_dict); // Increments refcount for us + } + + // Set the entry in the groups dictionary + playerc_blackboard___set_nested_dictionary_entry__(device, groups_dict, entry.key, entry.group, entry_dict); // Steals reference to entry_dict +} + +// Returns a list of dictionary objects containing the entries. +// Requires that set_queue_events is set to True for any events to be queued. +// All events will be retrieved. +// Will always return a list object. +%feature("autodoc", "1"); +PyObject *GetEvents() +{ + PyObject *list, *copy, *item; + int i, j, ok; + assert(self->py_private); + list = (PyObject*)PyTuple_GetItem(self->py_private, LIST_EVENTS_INDEX); // Borrowed reference + assert(list); - list = (PyObject*)PyTuple_GetItem(self->py_private,1); - copy = PyList_New(0); j = PyList_Size(list); + copy = PyList_New(j); // New reference + for (i = 0; i < j; i++) { - PyList_Append(copy, PyList_GetItem(list, 0)); + item = PyList_GetItem(list, 0); // Borrowed reference + Py_INCREF(item); + assert(item); + ok = PyList_SetItem(copy, i, item); // Steals reference + if (ok != 0) + { + PyErr_SetString(PyExc_RuntimeError, "Failed to set list entry"); + Py_XDECREF(copy); + Py_XDECREF(item); + return NULL; + } PySequence_DelItem(list,0); } + return copy; } -void set_queue_events(PyObject *boolean) +// Returns a dictionary of the entry dictionary objects indexed by group. +// Will only contain the latest values. +// Will always return a dictionary object. +PyObject *GetDict() { + PyObject *dict, *copy; + + assert(self->py_private); + dict = (PyObject*)PyTuple_GetItem(self->py_private, DICT_GROUPS_INDEX); // Borrowed reference + assert(dict); + + copy = PyDict_Copy(dict); // New reference + return copy; +} + +// Set whether events should be put into the list, to be retrieved by get_events_list(). +PyObject *SetQueueEvents(PyObject *boolean) +{ + int ok; + if(!PyBool_Check(boolean)) { - PyErr_SetString(PyExc_RuntimeError, "Expected a bool object."); - return; + PyErr_SetString(PyExc_TypeError, "Expected type 'bool'"); + return NULL; } - if (boolean==Py_False) + if (boolean == Py_False) { - PyTuple_SetItem(self->py_private,2,PyLong_FromLong(0)); + ok = PyTuple_SetItem((PyObject*)self->py_private, BOOL_QUEUE_EVENTS_INDEX, PyLong_FromLong(0)); // Steals reference + if (ok != 0) + { + PyErr_SetString(PyExc_RuntimeError, "Failed to set tuple entry"); + return NULL; + } } else { - PyTuple_SetItem(self->py_private,2,PyLong_FromLong(1)); + ok = PyTuple_SetItem((PyObject*)self->py_private, BOOL_QUEUE_EVENTS_INDEX, PyLong_FromLong(1)); // Steals reference + if (ok != 0) + { + PyErr_SetString(PyExc_RuntimeError, "Failed to set tuple entry"); + return NULL; + } } + + Py_RETURN_NONE; } -void unsubscribe_from_key_py(const char *key, const char *group) +PyObject *Subscribe(int access) { - long refcount; - PyObject *group_dict, *entry_dict, *groups; + int result; + result = playerc_blackboard_subscribe(self, access); + return PyInt_FromLong(result); +} - groups = (PyObject *)PyTuple_GetItem(self->py_private,0); - assert(groups); +PyObject *Unsubscribe() +{ + int result; + result = playerc_blackboard_unsubscribe(self); + return PyInt_FromLong(result); +} - group_dict = (PyObject*) PyDict_GetItemString(groups, group); - if (!group_dict) - { - PyErr_SetString(PyExc_RuntimeError, "Group does not exist"); - return; - } - - // find our key in the group, if it exists - entry_dict = (PyObject*) PyDict_GetItemString(group_dict, key); - if (!entry_dict) - { - PyErr_SetString(PyExc_RuntimeError, "Key does not exist"); - return; - } - refcount = PyLong_AsLong(PyDict_GetItemString(entry_dict, "___refcount")); - if(refcount==0) - { - PyErr_SetString(PyExc_RuntimeError, "No subscription to key to unsubscribe from."); - return; - } - if(refcount==1) - { - playerc_blackboard_unsubscribe_from_key(self, key, group); - } - PyDict_SetItemString(entry_dict, "___refcount", PyLong_FromLong(refcount-1)); +PyObject *SetString(const char* key, const char* group, const char* value) +{ + int result; + result = playerc_blackboard_set_string(self, key, group, value); + return PyInt_FromLong(result); } -PyObject * subscribe_to_key_py(const char *key, const char *group) +PyObject *SetInt(const char* key, const char* group, const int value) { - char *str; - int i, result, refcount; - double d; - PyObject *data, *entry_dict, *group_dict, *groups; - player_blackboard_entry_t *entry; + int result; + result = playerc_blackboard_set_int(self, key, group, value); + return PyInt_FromLong(result); +} - groups = (PyObject *)PyTuple_GetItem(self->py_private,0); - assert(groups); - // grab our group dict, if it doesnt exist create a new one - group_dict = (PyObject*) PyDict_GetItemString(groups, group); - if (!group_dict) - { - group_dict = PyDict_New(); - assert (group_dict); - result = PyDict_SetItemString(groups,group,group_dict); - if (result != 0) - { - PyErr_SetString(PyExc_RuntimeError, "Failed to set group key"); - return Py_None; - } - } - // find our key in the group, if it exists, create it if not - entry_dict = (PyObject*) PyDict_GetItemString(group_dict, key); - if (!entry_dict) - { - entry_dict = PyDict_New(); - assert (entry_dict); - PyDict_SetItemString(entry_dict, "key", PyString_FromString(key)); - PyDict_SetItemString(entry_dict, "group", PyString_FromString(group)); - PyDict_SetItemString(entry_dict, "___refcount", PyLong_FromLong(1)); - } - else - { - // key already exists, so increment our ref count and return current value - refcount = PyLong_AsLong(PyDict_GetItemString(entry_dict, "___refcount")); - if(refcount>=1000) - { - PyErr_SetString(PyExc_RuntimeError, "Reached over 1000 subscriptions"); - } - if(refcount!=0) - { - PyDict_SetItemString(entry_dict, "___refcount", PyLong_FromLong(refcount+1)); - Py_INCREF(entry_dict); - return entry_dict; - } - } - assert(self); - result = playerc_blackboard_subscribe_to_key(self, key, group, &entry); - if (result != 0) - { - PyErr_SetString(PyExc_RuntimeError, "Failed to subscribe to key"); - return Py_None; - } - assert(entry); - assert(entry->key); - assert(entry->key_count > 0); - PyDict_SetItemString(entry_dict, "type", PyLong_FromLong(entry->type)); - PyDict_SetItemString(entry_dict, "subtype", PyLong_FromLong(entry->subtype)); - PyDict_SetItemString(entry_dict, "timestamp_sec", PyLong_FromLong(entry->timestamp_sec)); - PyDict_SetItemString(entry_dict, "timestamp_usec", PyLong_FromLong(entry->timestamp_usec)); - switch(entry->subtype) +PyObject *SetDouble(const char* key, const char* group, const double value) +{ + int result; + result = playerc_blackboard_set_double(self, key, group, value); + return PyInt_FromLong(result); +} + +int UnsubscribeFromKey(const char *key, const char *group) +{ + PyObject *groups_dict, *group_dict, *subscription_data; + int result, ref_count; + + subscription_data = PyTuple_GetItem((PyObject*)self->py_private, DICT_SUBSCRIPTION_DATA_INDEX); // Borrowed reference + assert(subscription_data); + ref_count = playerc_blackboard___increment_reference_count__(self, subscription_data, key, group, -1); + if (ref_count <= 0) + { + groups_dict = PyTuple_GetItem((PyObject*)self->py_private, DICT_GROUPS_INDEX); // Borrowed reference + assert(groups_dict); + group_dict = PyDict_GetItemString(groups_dict, group); // Borrowed reference + assert(group_dict); + PyDict_DelItemString(group_dict, key); + } + + result = playerc_blackboard_unsubscribe_from_key(self, key, group); + return result; +} + +PyObject *SubscribeToKey(const char *key, const char *group) +{ + int result; + PyObject *subscription_data, *groups_dict, *entry_dict; + player_blackboard_entry_t *entry; + + result = playerc_blackboard_subscribe_to_key(self, key, group, &entry); + if (result != 0) + { + PyErr_SetString(PyExc_RuntimeError, "Failed to subscribe to key"); + return NULL; + } + + entry_dict = playerc_blackboard___convert_blackboard_entry__(self, entry); // New reference + assert(entry_dict); + groups_dict = (PyObject*)PyTuple_GetItem((PyObject*)self->py_private, DICT_GROUPS_INDEX); // Borrowed reference + assert(groups_dict); + + Py_INCREF(entry_dict); + playerc_blackboard___set_nested_dictionary_entry__(self, groups_dict, key, group, entry_dict); // Steals reference to entry_dict + + subscription_data = PyTuple_GetItem((PyObject*)self->py_private, DICT_SUBSCRIPTION_DATA_INDEX); // Borrowed reference + assert(subscription_data); + + playerc_blackboard___increment_reference_count__(self, subscription_data, key, group, 1); + + free(entry->key); + free(entry->group); + free(entry->data); + free(entry); + + return entry_dict; +} + +int UnsubscribeFromGroup(const char *group) +{ + int i, j, ref_count; + PyObject *groups_dict, *group_dict, *list, *tuple, *subscription_data, *items, *item; + const char* key; + int result; + + result = playerc_blackboard_unsubscribe_from_group(self, group); + + assert(self->py_private); + groups_dict = PyTuple_GetItem((PyObject*)self->py_private, DICT_GROUPS_INDEX); // Borrowed reference + assert(groups_dict); + + group_dict = PyDict_GetItemString(groups_dict, group); // Borrowed reference + if (group_dict) + { + list = PyList_New(0); // New reference + subscription_data = PyTuple_GetItem((PyObject*)self->py_private, DICT_SUBSCRIPTION_DATA_INDEX); // Borrowed reference + assert(subscription_data); + + items = PyDict_Items(group_dict); + j = PyList_Size(items); + for (i = 0; i < j; i++) { - case PLAYERC_BLACKBOARD_DATA_SUBTYPE_NONE: - data = Py_None; - break; - case PLAYERC_BLACKBOARD_DATA_SUBTYPE_STRING: - assert(entry->type == PLAYERC_BLACKBOARD_DATA_TYPE_COMPLEX); - str = malloc(entry->data_count); - assert(str); - memcpy(str, entry->data, entry->data_count); - data = PyString_FromString(str); - break; - case PLAYERC_BLACKBOARD_DATA_SUBTYPE_INT: - assert(entry->type == PLAYERC_BLACKBOARD_DATA_TYPE_SIMPLE); - i = 0; - memcpy(&i, entry->data, entry->data_count); - data = PyLong_FromLong(i); - break; - case PLAYERC_BLACKBOARD_DATA_SUBTYPE_DOUBLE: - assert(entry->type == PLAYERC_BLACKBOARD_DATA_TYPE_SIMPLE); - d = 0.0; - memcpy(&d, entry->data, entry->data_count); - data = PyFloat_FromDouble(d); - break; - default: - data = Py_None; - break; + tuple = PyList_GetItem(items, 0); // Borrowed reference + assert(tuple); + item = PyTuple_GetItem(tuple, 0); // Borrowed reference + assert(item); + key = PyString_AsString(item); + ref_count = playerc_blackboard___increment_reference_count__(self, subscription_data, key, group, 0); + if (ref_count <= 0) + { + PyList_Append(list, item); // Increments refcount for us + } + } + j = PyList_Size(list); + for (i =0; i < j; i++) + { + item = PyList_GetItem(list, i); // Borrowed reference + assert(item); + PyDict_DelItem(group_dict, item); } - PyDict_SetItemString(entry_dict, "data", data); - result = PyDict_SetItemString(group_dict,key,entry_dict); - if (result != 0) - { - PyErr_SetString(PyExc_RuntimeError, "Failed to create key dictionary entry"); - return Py_None; - } - free(entry->key); - free(entry->group); - free(entry->data); - free(entry); - Py_INCREF(entry_dict); - return entry_dict; + + Py_DECREF(list); } -PyObject * subscribe_to_key(const char *key, const char *group) + return result; +} + +int SubscribeToGroup(const char *group) { - return playerc_blackboard_subscribe_to_key_py( self, key, group); + int result; + result = playerc_blackboard_subscribe_to_group(self, group); + return result; } -PyObject * get_value(const char *key, const char *group) +PyObject *GetEntry(const char *key, const char *group) { - PyObject *group_dict, *entry_dict, *groups; - int refcount; - - groups = (PyObject *)PyTuple_GetItem(self->py_private,0); - assert(groups); + int result; + PyObject *entry_dict, *groups_dict; + player_blackboard_entry_t *entry; + + result = playerc_blackboard_get_entry(self, key, group, &entry); + if (result != 0) + { + PyErr_SetString(PyExc_RuntimeError, "Failed to get entry"); + return NULL; + } + entry_dict = playerc_blackboard___convert_blackboard_entry__(self, entry); // New reference + + groups_dict = PyTuple_GetItem((PyObject*)self->py_private, DICT_GROUPS_INDEX); // Borrowed reference + assert(groups_dict); + assert(entry_dict); + Py_INCREF(entry_dict); + playerc_blackboard___set_nested_dictionary_entry__(self, groups_dict, key, group, entry_dict); // Steals reference to entry_dict - group_dict = (PyObject*) PyDict_GetItemString(groups, group); - if (!group_dict) - return playerc_blackboard_subscribe_to_key_py( self, key, group); - // find our key in the group, if it exists - entry_dict = (PyObject*) PyDict_GetItemString(group_dict, key); - if (!entry_dict) - return playerc_blackboard_subscribe_to_key_py( self, key, group); - refcount = PyLong_AsLong(PyDict_GetItemString(entry_dict, "___refcount")); - if(refcount==0) - return playerc_blackboard_subscribe_to_key_py( self, key, group); - Py_INCREF(entry_dict); - return entry_dict; + free(entry->key); + free(entry->group); + free(entry->data); + free(entry); + + return entry_dict; } + +PyObject *SetEntryRaw(player_blackboard_entry_t *entry) +{ + int result; + PyObject *entry_dict, *groups_dict; - void set_entry_raw(player_blackboard_entry_t *entry) + entry_dict = playerc_blackboard___convert_blackboard_entry__(self, entry); // New reference + assert(entry_dict); + groups_dict = PyTuple_GetItem((PyObject*)self->py_private, DICT_GROUPS_INDEX); // Borrowed reference + assert(groups_dict); + + playerc_blackboard___set_nested_dictionary_entry__(self, groups_dict, entry->key, entry->group, entry_dict); // Steals reference to entry_dict + result = playerc_blackboard_set_entry(self, entry); + return PyInt_FromLong(result); +} + +PyObject *SetEntry(PyObject *dict) +{ + PyObject *key, *group, *type, *subtype, *timestamp_sec, *timestamp_usec, *data, *groups_dict; + char *str; + int i, result, length; + double d; + + if (!PyDict_Check(dict)) { - playerc_blackboard_set_entry(self,entry); + PyErr_SetString(PyExc_RuntimeError, "Expected a dictionary object."); + return NULL; } - PyObject *set_entry(PyObject *dict) - { - PyObject *key; - PyObject *group; - PyObject *type; - PyObject *subtype; - PyObject *timestamp_sec; - PyObject *timestamp_usec; - PyObject *data; - char *str; - int i, result; - double d; - int length; + player_blackboard_entry_t entry; + memset(&entry, 0, sizeof(entry)); - if (!PyDict_Check(dict)) - { - PyErr_SetString(PyExc_RuntimeError, "Expected a dictionary object."); - return PyLong_FromLong(-1); - } + key = PyDict_GetItem(dict, PyString_FromString("key")); // Borrowed reference + group = PyDict_GetItem(dict, PyString_FromString("group")); // Borrowed reference + type = PyDict_GetItem(dict, PyString_FromString("type")); // Borrowed reference + subtype = PyDict_GetItem(dict, PyString_FromString("subtype")); // Borrowed reference + timestamp_sec = PyDict_GetItem(dict, PyString_FromString("timestamp_sec")); // Borrowed reference + timestamp_usec = PyDict_GetItem(dict, PyString_FromString("timestamp_usec")); // Borrowed reference + data = PyDict_GetItem(dict, PyString_FromString("data")); // Borrowed reference - player_blackboard_entry_t entry; - memset(&entry, 0, sizeof(entry)); + if (key == NULL || group == NULL || type == NULL || subtype == NULL || timestamp_sec == NULL || timestamp_usec == NULL || data == NULL) + { + PyErr_SetString(PyExc_RuntimeError, "Dictionary object missing keys. One or more of the following keys were not found: 'key', 'group', 'type', 'subtype', 'timestamp_sec', 'timestamp_usec', 'data'."); + return NULL; + } - key = PyDict_GetItem(dict, PyString_FromString("key")); - group = PyDict_GetItem(dict, PyString_FromString("group")); - type = PyDict_GetItem(dict, PyString_FromString("type")); - subtype = PyDict_GetItem(dict, PyString_FromString("subtype")); - timestamp_sec = PyDict_GetItem(dict, PyString_FromString("timestamp_sec")); - timestamp_usec = PyDict_GetItem(dict, PyString_FromString("timestamp_usec")); - data = PyDict_GetItem(dict, PyString_FromString("data")); + if (!PyString_Check(key)) + { + PyErr_SetString(PyExc_TypeError, "'key' should be a 'string' type."); + return NULL; + } - if (key == NULL || group == NULL || type == NULL || subtype == NULL || timestamp_sec == NULL || timestamp_usec == NULL || data == NULL) - { - PyErr_SetString(PyExc_RuntimeError, "Dictionary object missing keys."); - return PyLong_FromLong(-1); - } + if (!PyString_Check(group)) + { + PyErr_SetString(PyExc_TypeError, "'group' should be a 'string' type."); + return NULL; + } - if (!PyString_Check(key)) - { - PyErr_SetString(PyExc_RuntimeError, "key should be a string type."); - return PyLong_FromLong(-1); - } + if (!PyLong_Check(type)) + { + PyErr_SetString(PyExc_TypeError, "'type' should be a 'long' type."); + return NULL; + } - if (!PyString_Check(group)) - { - PyErr_SetString(PyExc_RuntimeError, "group should be a string type."); - return PyLong_FromLong(-1); - } + if (!PyLong_Check(subtype)) + { + PyErr_SetString(PyExc_TypeError, "'subtype' should be a 'long' type."); + return PyLong_FromLong(-1); + } - if (!PyLong_Check(type)) - { - PyErr_SetString(PyExc_RuntimeError, "type should be a long type."); - return PyLong_FromLong(-1); - } + if (!PyLong_Check(timestamp_sec)) + { + PyErr_SetString(PyExc_TypeError, "'timestamp_sec' should be a 'long' type."); + return NULL; + } - if (!PyLong_Check(subtype)) - { - PyErr_SetString(PyExc_RuntimeError, "subtype should be a long type."); - return PyLong_FromLong(-1); - } + if (!PyLong_Check(timestamp_usec)) + { + PyErr_SetString(PyExc_TypeError, "'timestamp_usec' should be a 'long' type"); + return NULL; + } - if (!PyLong_Check(timestamp_sec)) - { - PyErr_SetString(PyExc_RuntimeError, "timestamp_sec should be a long type."); - return PyLong_FromLong(-1); - } + entry.key = PyString_AsString(key); + entry.key_count = strlen(entry.key) + 1; + entry.group = PyString_AsString(key); + entry.group_count = strlen(entry.group) + 1; + entry.type = PyInt_AsLong(type); + entry.subtype = PyInt_AsLong(subtype); + entry.timestamp_sec = PyInt_AsLong(timestamp_sec); + entry.timestamp_usec = PyInt_AsLong(timestamp_usec); - if (!PyLong_Check(timestamp_usec)) - { - PyErr_SetString(PyExc_RuntimeError, "timestamp_usec should be a long type"); - return PyLong_FromLong(-1); - } + switch (entry.subtype) + { + case PLAYERC_BLACKBOARD_DATA_SUBTYPE_NONE: + entry.data = NULL; + entry.data_count = 0; + break; + case PLAYERC_BLACKBOARD_DATA_SUBTYPE_STRING: + if (!PyString_Check(data)) + { + PyErr_SetString(PyExc_TypeError, "'data' should be a 'string' type."); + return NULL; + } + str = NULL; + str = PyString_AsString(data); + length = strlen(str) + 1; + entry.data = malloc(length); + assert(entry.data); + memcpy(entry.data, str, length); + entry.data_count = length; + break; + case PLAYERC_BLACKBOARD_DATA_SUBTYPE_INT: + if (!PyLong_Check(data)) + { + PyErr_SetString(PyExc_TypeError, "'data' should be a 'long' type."); + return NULL; + } + i = 0; + i = PyInt_AsLong(data); + length = sizeof(i); + entry.data = malloc(length); + assert(entry.data); + memcpy(&entry.data, &i, length); + entry.data_count = length; + break; + case PLAYERC_BLACKBOARD_DATA_SUBTYPE_DOUBLE: + if (!PyLong_Check(data)) + { + PyErr_SetString(PyExc_TypeError, "'data' should be a 'long' type."); + return NULL; + } + d = 0.0; + d = PyLong_AsDouble(data); + length = sizeof(d); + entry.data = malloc(length); + assert(entry.data); + memcpy(&entry.data, &d, length); + entry.data_count = length; + break; + default: + entry.data = NULL; + entry.data_count = 0; + break; + } - entry.key = PyString_AsString(key); - entry.key_count = strlen(entry.key) + 1; - entry.group = PyString_AsString(key); - entry.group_count = strlen(entry.group) + 1; - entry.type = PyInt_AsLong(type); - entry.subtype = PyInt_AsLong(subtype); - entry.timestamp_sec = PyInt_AsLong(timestamp_sec); - entry.timestamp_usec = PyInt_AsLong(timestamp_usec); + result = playerc_blackboard_set_entry(self, &entry); + groups_dict = PyTuple_GetItem((PyObject*)self->py_private, DICT_GROUPS_INDEX); // Borrowed reference + assert(groups_dict); + Py_INCREF(dict); + playerc_blackboard___set_nested_dictionary_entry__(self, groups_dict, entry.key, entry.group, dict); // Steals reference to entry + free(entry.data); - switch (entry.subtype) - { - case PLAYERC_BLACKBOARD_DATA_SUBTYPE_NONE: - entry.data = NULL; - entry.data_count = 0; - break; - case PLAYERC_BLACKBOARD_DATA_SUBTYPE_STRING: - if (!PyString_Check(data)) - { - PyErr_SetString(PyExc_RuntimeError, "data should be a string type."); - return PyLong_FromLong(-1); - } - str = NULL; - str = PyString_AsString(data); - length = strlen(str) + 1; - entry.data = malloc(length); - assert(entry.data); - memcpy(entry.data, str, length); - entry.data_count = length; - break; - case PLAYERC_BLACKBOARD_DATA_SUBTYPE_INT: - if (!PyLong_Check(data)) - { - PyErr_SetString(PyExc_RuntimeError, "data should be a long type."); - return PyLong_FromLong(-1); - } - i = 0; - i = PyInt_AsLong(data); - length = sizeof(i); - entry.data = malloc(length); - assert(entry.data); - memcpy(&entry.data, &i, length); - entry.data_count = length; - break; - case PLAYERC_BLACKBOARD_DATA_SUBTYPE_DOUBLE: - if (!PyLong_Check(data)) - { - PyErr_SetString(PyExc_RuntimeError, "data should be a long type."); - return PyLong_FromLong(-1); - } - d = 0.0; - d = PyLong_AsDouble(data); - length = sizeof(d); - entry.data = malloc(length); - assert(entry.data); - memcpy(&entry.data, &d, length); - entry.data_count = length; - break; - default: - entry.data = NULL; - entry.data_count = 0; - break; - } - result = playerc_blackboard_set_entry(self, &entry); - if (result != 0) - { - return PyLong_FromLong(-1); - } - free(entry.data); + return PyInt_FromLong(result); +} - return PyLong_FromLong(0); - } } %{ // This code overrides the default swig wrapper. We use a different callback function for the python interface. -// It performs the same functionality, but points the callback function to the new python one. +// It performs the same functionality, but points the callback function to the new python one. playerc_blackboard_t *python_playerc_blackboard_create(playerc_client_t *client, int index) { playerc_blackboard_t *device= playerc_blackboard_create(client, index); @@ -850,10 +1090,14 @@ PyErr_SetString(PyExc_RuntimeError, "Failed to create blackboard"); return NULL; } + + // Tuple item 0: Dictionary of group/keys/values. As in dict['group']['key']=value. Updated on client.read() callback. + // Tuple item 1: Dictionary of keys that have been subscribed to. + // Tuple item 2: List of all events that have occurred. Need to call set_queue_events(True). Updated on client.read() callback. + // Tuple item 3: Whether to queue all events or not. Default not. (0 for false, non-zero for true) + device->py_private = (void*)Py_BuildValue("({},{},[],i)",0); // tuple of a dict and a list to store our keys and events in - device->py_private = Py_BuildValue("({},[],i)",0); // tuple of a dict and a list to store our keys and events in - - device->on_blackboard_event = playerc_blackboard_python_on_blackboard_event; + device->on_blackboard_event = playerc_blackboard___python_on_blackboard_event__; return device; } @@ -866,7 +1110,7 @@ playerc_device_term(&device->info); if (device->py_private != NULL) { - Py_DECREF((PyObject*)device->py_private); + Py_XDECREF((PyObject*)device->py_private); } free(device); } Modified: code/player/trunk/client_libs/libplayerc/dev_blackboard.c =================================================================== --- code/player/trunk/client_libs/libplayerc/dev_blackboard.c 2008-07-23 06:40:18 UTC (rev 6912) +++ code/player/trunk/client_libs/libplayerc/dev_blackboard.c 2008-07-23 07:14:31 UTC (rev 6913) @@ -295,6 +295,43 @@ return 0; } +// Get the value of a key +int playerc_blackboard_get_entry(playerc_blackboard_t* device, const char* key, const char* group, player_blackboard_entry_t** entry_out) +{ + player_blackboard_entry_t req; + memset(&req, 0, sizeof(req)); + req.key = strdup(key); + req.key_count = strlen(key) + 1; + + req.group = strdup(group); + req.group_count = strlen(group) + 1; + + if (playerc_client_request(device->info.client, &device->info, + PLAYER_BLACKBOARD_REQ_GET_ENTRY, &req, (void**)entry_out) < 0) + { + if (req.key != NULL) + { + free(req.key); + } + if (req.group != NULL) + { + free(req.group); + } + PLAYERC_ERR("failed to get to blackboard entry"); + return -1; + } + + if (req.key != NULL) + { + free(req.key); + } + if (req.group != NULL) + { + free(req.group); + } + return 0; +} + // Unsubscribe from a blackboard key int playerc_blackboard_unsubscribe_from_key(playerc_blackboard_t* device, const char* key, const char* group) { Modified: code/player/trunk/client_libs/libplayerc/playerc.h =================================================================== --- code/player/trunk/client_libs/libplayerc/playerc.h 2008-07-23 06:40:18 UTC (rev 6912) +++ code/player/trunk/client_libs/libplayerc/playerc.h 2008-07-23 07:14:31 UTC (rev 6913) @@ -1174,6 +1174,10 @@ * responsible for freeing it. */ int playerc_blackboard_subscribe_to_key(playerc_blackboard_t *device, const char* key, const char* group, player_blackboard_entry_t** entry); +/** @brief Get the current value of a key, without subscribing. If entry is none null it will be filled in with the response. The caller is + * responsible for freeing it. */ +int playerc_blackboard_get_entry(playerc_blackboard_t *device, const char* key, const char* group, player_blackboard_entry_t** entry); + /** @brief Unsubscribe from a key. */ int playerc_blackboard_unsubscribe_from_key(playerc_blackboard_t *device, const char* key, const char* group); Modified: code/player/trunk/libplayercore/interfaces/064_blackboard.def =================================================================== --- code/player/trunk/libplayercore/interfaces/064_blackboard.def 2008-07-23 06:40:18 UTC (rev 6912) +++ code/player/trunk/libplayercore/interfaces/064_blackboard.def 2008-07-23 07:14:31 UTC (rev 6913) @@ -13,6 +13,8 @@ message { REQ, SUBSCRIBE_TO_GROUP, 4, player_blackboard_entry_t }; /** Request/reply subtype: unsubscribe from group. */ message { REQ, UNSUBSCRIBE_FROM_GROUP, 5, player_blackboard_entry_t }; +/** Request/reply subtype: get a value. */ +message { REQ, GET_ENTRY, 6, player_blackboard_entry_t }; + /** Data update reply */ message { DATA, UPDATE, 1, player_blackboard_entry_t }; - Modified: code/player/trunk/server/drivers/blackboard/localbb/localbb.cpp =================================================================== --- code/player/trunk/server/drivers/blackboard/localbb/localbb.cpp 2008-07-23 06:40:18 UTC (rev 6912) +++ code/player/trunk/server/drivers/blackboard/localbb/localbb.cpp 2008-07-23 07:14:31 UTC (rev 6913) @@ -69,12 +69,14 @@ #include <stdlib.h> #include <libplayercore/playercore.h> #include <libplayercore/error.h> -// #include <libplayerc++/playererror.h> #include <vector> #include <map> #include <strings.h> #include <iostream> +bool operator ==(const player_devaddr_t &a, const player_devaddr_t &b); +bool operator <(const player_devaddr_t &a, const player_devaddr_t &b); + /**@struct EntryData * @brief Custom blackboard-entry data representation used internally by the driver. */ typedef struct EntryData @@ -180,6 +182,8 @@ int Setup(); /** @brief Driver de-initialisation. */ int Shutdown(); + /** Override the unsubscribe to stop sending events to devices which are no longer subscribed. */ + int Unsubscribe(player_devaddr_t addr); // MessageHandler /** @brief Process incoming messages. @@ -203,6 +207,15 @@ * @return 0 for success, -1 on error. */ int ProcessSubscribeKeyMessage(QueuePointer &resp_queue, player_msghdr * hdr, void * data); + /** @brief Process a get entry message. + * Retrieves the entry for the given key, but does not subscribe to the key. + * Publishes the entry. + * @param resp_queue Player response queue. + * @param hdr Message header. + * @param data Message data. + * @return 0 for success, -1 on error. + * */ + int ProcessGetEntryMessage(QueuePointer &resp_queue, player_msghdr * hdr, void * data); /** @brief Process an unsubscribe from key message. * Removes the response queue from the list of devices listening to that key in the map. * @param resp_queue Player response queue. @@ -245,7 +258,7 @@ * @param group Second identifier. * @param resp_queue Player response queue of the subscriber. */ - BlackBoardEntry SubscribeKey(const string &key, const string &group, const QueuePointer &resp_queue); + BlackBoardEntry SubscribeKey(const string &key, const string &group, const QueuePointer &resp_queue, const player_devaddr_t addr); /** @brief Remove the key and queue combination from the listeners hash-map. * @param key Entry key. * @param group Second identifier. @@ -258,7 +271,7 @@ * @param qp resp_queue Player response queue of the subscriber * @return Vector of blackboard entries of that group */ - vector<BlackBoardEntry> SubscribeGroup(const string &group, const QueuePointer &qp); + vector<BlackBoardEntry> SubscribeGroup(const string &group, const QueuePointer &qp, const player_devaddr_t addr); /** * @brief Remove the group from the group listeners hash-map. * @param group Entry group @@ -267,8 +280,8 @@ void UnsubscribeGroup(const string &group, const QueuePointer &qp); /** @brief Set the entry in the entries hashmap. * - * @param entry BlackBoardEntry that must be put in the hashmap. - */ + * @param entry BlackBoardEntry that must be put in the hashmap. + */ void SetEntry(const BlackBoardEntry &entry); // Helper functions @@ -281,17 +294,80 @@ /** Map of labels to entry data. * map<group, map<key, entry> > */ + map<string, map<string, BlackBoardEntry> > entries; /** Map of labels to listening queues. * map<group, map<key, vector<device queue> > > */ map<string, map<string, vector<QueuePointer> > > listeners; + /** Map of groups to queues subscribed to groups. * map<group, vector<device queue> > */ map<string, vector<QueuePointer> > group_listeners; + + /** Map of queues to devices. Used to remove unneeded queues when a device is unsubscribed. */ + map<player_devaddr_t, QueuePointer> subscribed_devices; }; +//////////////////////////////////////////////////////////////////////////////// +// Override the unsubscribe. Stop sending out events to unsubscribed devices. +int LocalBB::Unsubscribe(player_devaddr_t addr) +{ + QueuePointer &qp = subscribed_devices[addr]; + for (map<string, map<string, vector<QueuePointer> > >::iterator itr = listeners.begin(); itr != listeners.end(); itr++) + { + map<string, vector<QueuePointer> > &keys = (*itr).second; + for (map<string, vector<QueuePointer> >::iterator jtr = keys.begin(); jtr != keys.end(); jtr++) + { + vector<QueuePointer> &qps = (*jtr).second; + vector<vector<QueuePointer>::iterator> remove_list; + for (vector<QueuePointer>::iterator ktr = qps.begin(); ktr != qps.end(); ktr++) + { + if ((*ktr) == qp) + { + remove_list.push_back(ktr); + } + } + for (vector<vector<QueuePointer>::iterator>::iterator ltr = remove_list.begin(); ltr != remove_list.end(); ltr++) + { + qps.erase(*ltr); + } + } + } + for (map<string, vector<QueuePointer> >::iterator itr = group_listeners.begin(); itr != group_listeners.end(); itr++) + { + vector<QueuePointer> &qps = (*itr).second; + vector<vector<QueuePointer>::iterator> remove_list; + for (vector<QueuePointer>::iterator jtr = qps.begin(); jtr != qps.end(); jtr++) + { + if ((*jtr) == qp) + { + remove_list.push_back(jtr); + } + } + for (vector<vector<QueuePointer>::iterator>::iterator ltr = remove_list.begin(); ltr != remove_list.end(); ltr++) + { + qps.erase(*ltr); + } + } + int shutdownResult; + + if(subscriptions == 0) + shutdownResult = -1; + else if ( subscriptions == 1) + { + shutdownResult = Shutdown(); + subscriptions--; + } + else + { + subscriptions--; + shutdownResult = 0; + } + + return shutdownResult; +} //////////////////////////////////////////////////////////////////////////////// // Factory method. Driver* LocalBB_Init(ConfigFile* cf, int section) @@ -364,6 +440,10 @@ { return ProcessUnsubscribeGroupMessage(resp_queue, hdr, data); } + else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_BLACKBOARD_REQ_GET_ENTRY, this->device_addr)) + { + return ProcessGetEntryMessage(resp_queue, hdr, data); + } // Don't know how to handle this message return -1; } @@ -377,7 +457,7 @@ // Add the device to the listeners map player_blackboard_entry_t *request = reinterpret_cast<player_blackboard_entry_t*>(data); - BlackBoardEntry current_value = SubscribeKey(request->key, request->group, resp_queue); + BlackBoardEntry current_value = SubscribeKey(request->key, request->group, resp_queue, hdr->addr); // Get the entry for the given key player_blackboard_entry_t response = ToPlayerBlackBoardEntry(current_value); @@ -408,7 +488,47 @@ return 0; } +//////////////////////////////////////////////////////////////////////////////// +// Retrieve an entry for a key. +int LocalBB::ProcessGetEntryMessage(QueuePointer &resp_queue, player_msghdr * hdr, void * data) +{ + if (!CheckHeader(hdr)) + return -1; + // Retrieve the entry for the key + player_blackboard_entry_t *request = reinterpret_cast<player_blackboard_entry_t*>(data); + BlackBoardEntry current_value = entries[string(request->group)][string(request->key)]; + + // Convert the entry + player_blackboard_entry_t response = ToPlayerBlackBoardEntry(current_value); + size_t response_size = sizeof(player_blackboard_entry_t) + response.key_count + response.group_count + response.data_count; + + // Publish the blackboard entry + this->Publish( + this->device_addr, + resp_queue, + PLAYER_MSGTYPE_RESP_ACK, + PLAYER_BLACKBOARD_REQ_GET_ENTRY, + &response, + response_size, + NULL); + + if (response.key) + { + delete [] response.key; + } + if (response.group) + { + delete [] response.group; + } + if (response.data) + { + delete [] response.data; + } + + return 0; +} + //////////////////////////////////////////////////////////////////////////////// // Unsubscribe a device from a key. int LocalBB::ProcessUnsubscribeKeyMessage(QueuePointer &resp_queue, player_msghdr * hdr, void * data) @@ -438,14 +558,13 @@ int LocalBB::ProcessSubscribeGroupMessage(QueuePointer &resp_queue, player_msghdr * hdr, void * data) { if (!CheckHeader(hdr)) - return -1; + return -1; // Add the device to the listeners map player_blackboard_entry_t *request = reinterpret_cast<player_blackboard_entry_t*>(data); - vector<BlackBoardEntry> current_values = SubscribeGroup(request->group, resp_queue); + vector<BlackBoardEntry> entries = SubscribeGroup(request->group, resp_queue, hdr->addr); - // Get the entries for the given key and send the data updates - for (vector<BlackBoardEntry>::iterator itr=current_values.begin(); itr != current_values.end(); itr++) + for (vector<BlackBoardEntry>::iterator itr = entries.begin(); itr != entries.end(); itr++) { BlackBoardEntry current_value = *itr; player_blackboard_entry_t response = ToPlayerBlackBoardEntry(current_value); @@ -503,7 +622,7 @@ this->device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, - PLAYER_BLACKBOARD_REQ_UNSUBSCRIBE_FROM_KEY, + PLAYER_BLACKBOARD_REQ_UNSUBSCRIBE_FROM_GROUP, NULL, 0, NULL); @@ -566,9 +685,10 @@ //////////////////////////////////////////////////////////////////////////////// // Add a device to the listener list for a key. Return the current value of the entry. -BlackBoardEntry LocalBB::SubscribeKey(const string &key, const string &group, const QueuePointer &resp_queue) +BlackBoardEntry LocalBB::SubscribeKey(const string &key, const string &group, const QueuePointer &resp_queue, const player_devaddr_t addr) { listeners[group][key].push_back(resp_queue); + subscribed_devices[addr] = resp_queue; BlackBoardEntry entry = entries[group][key]; if (entry.key == "") { @@ -596,17 +716,27 @@ //////////////////////////////////////////////////////////////////////////////// // Add a device to the group listener map. Return vector of entries for that group. -vector<BlackBoardEntry> LocalBB::SubscribeGroup(const string &group, const QueuePointer &qp) +vector<BlackBoardEntry> LocalBB::SubscribeGroup(const string &group, const QueuePointer &qp, const player_devaddr_t addr) { group_listeners[group].push_back(qp); + subscribed_devices[addr] = qp; + vector<BlackBoardEntry> group_entries; + //map<group, map<key, entry> > + map<string, BlackBoardEntry> &entry_map = entries[group]; - // Add all entries for a group to the group_entries vector - //map<group, map<key, entry> > - map<string, BlackBoardEntry> entry_map = entries[group]; for (map<string, BlackBoardEntry>::iterator itr = entry_map.begin(); itr != entry_map.end(); itr++) { - group_entries.push_back((*itr).second); + BlackBoardEntry current_value = (*itr).second; + if (current_value.key == "") + { + current_value.key = (*itr).first; + } + if (current_value.group == "") + { + current_value.group = group; + } + group_entries.push_back(current_value); } return group_entries; } @@ -645,3 +775,27 @@ } return true; } + + +bool operator ==(const player_devaddr_t &a, const player_devaddr_t &b) +{ + return (a.host == b.host) && (a.robot == b.robot) && (a.interf == b.interf) && (a.index == b.index); +} + +bool operator <(const player_devaddr_t &a, const player_devaddr_t &b) +{ + if (a.host != b.host) + { + return a.host < b.host; + } + else if (a.robot != b.robot) + { + return a.robot < b.robot; + } + else if (a.interf != b.interf) + { + return a.interf < b.interf; + } + + return a.index < b.index; +} This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2008-07-23 00:20:38
|
Revision: 6914 http://playerstage.svn.sourceforge.net/playerstage/?rev=6914&view=rev Author: thjc Date: 2008-07-23 07:20:46 +0000 (Wed, 23 Jul 2008) Log Message: ----------- Merged 6905 from 2-1 removed left over debug print in file watcher Modified Paths: -------------- code/player/trunk/libplayercore/filewatcher.cc Property Changed: ---------------- code/player/trunk/ Property changes on: code/player/trunk ___________________________________________________________________ Modified: svn:mergeinfo - /code/player/branches/release-2-1-patches:6672-6673,6738,6834,6886 + /code/player/branches/release-2-1-patches:6672-6673,6738,6834,6886,6905 Modified: code/player/trunk/libplayercore/filewatcher.cc =================================================================== --- code/player/trunk/libplayercore/filewatcher.cc 2008-07-23 07:14:31 UTC (rev 6913) +++ code/player/trunk/libplayercore/filewatcher.cc 2008-07-23 07:20:46 UTC (rev 6914) @@ -123,7 +123,6 @@ int FileWatcher::AddFileWatch(int fd, QueuePointer & queue, bool WatchRead, bool WatchWrite, bool WatchExcept) { Lock(); - fprintf(stderr,"Added file watch %d \n",fd); // find the first available file descriptor struct fd_driver_pair *next_entry = NULL; if (WatchedFilesArrayCount < WatchedFilesArraySize) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <gb...@us...> - 2008-08-11 19:56:50
|
Revision: 6950 http://playerstage.svn.sourceforge.net/playerstage/?rev=6950&view=rev Author: gbiggs Date: 2008-08-12 02:57:00 +0000 (Tue, 12 Aug 2008) Log Message: ----------- Fixed a bug in setting include dirs for drivers, cleaned up laser_config.h inclusion. Modified Paths: -------------- code/player/trunk/cmake/internal/DriverUtils.cmake code/player/trunk/server/drivers/laser/CMakeLists.txt code/player/trunk/server/drivers/laser/sicklms200.cc Modified: code/player/trunk/cmake/internal/DriverUtils.cmake =================================================================== --- code/player/trunk/cmake/internal/DriverUtils.cmake 2008-08-11 00:22:53 UTC (rev 6949) +++ code/player/trunk/cmake/internal/DriverUtils.cmake 2008-08-12 02:57:00 UTC (rev 6950) @@ -41,7 +41,7 @@ MESSAGE (STATUS "WARNING: No sources given for driver ${_name}") ENDIF (NOT _srcs) # Add this driver's list of sources to the list of sources for libplayerdrivers - PLAYERDRIVER_ADD_TO_BUILT (${_name} "${_includeDir}" "${_libDir}" "${_linkFlags}" "${_cFlags}" ${_srcs}) + PLAYERDRIVER_ADD_TO_BUILT (${_name} "${_includeDirs}" "${_libDir}" "${_linkFlags}" "${_cFlags}" ${_srcs}) ENDIF (${_cumulativeVar}) ENDMACRO (PLAYERDRIVER_ADD_DRIVER) Modified: code/player/trunk/server/drivers/laser/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/laser/CMakeLists.txt 2008-08-11 00:22:53 UTC (rev 6949) +++ code/player/trunk/server/drivers/laser/CMakeLists.txt 2008-08-12 02:57:00 UTC (rev 6950) @@ -1,16 +1,16 @@ -CHECK_INCLUDE_FILES (linux/serial.h HAVE_LINUX_SERIAL_H) -CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/laser_config.h.in ${CMAKE_CURRENT_BINARY_DIR}/laser_config.h) -include_directories(${CMAKE_CURRENT_BINARY_DIR}) - PLAYERDRIVER_OPTION (bumper2laser build_bumper2laser ON) PLAYERDRIVER_ADD_DRIVER (bumper2laser build_bumper2laser SOURCES bumper2laser.cc) PLAYERDRIVER_OPTION (pbslaser build_pbslaser ON) PLAYERDRIVER_ADD_DRIVER (pbslaser build_pbslaser SOURCES pbs_driver.cc) +CHECK_INCLUDE_FILES (linux/serial.h HAVE_LINUX_SERIAL_H) +SET (laser_config_h_in ${CMAKE_CURRENT_SOURCE_DIR}/laser_config.h.in) +SET (laser_config_h ${CMAKE_CURRENT_BINARY_DIR}/laser_config.h) +CONFIGURE_FILE (${CMAKE_CURRENT_SOURCE_DIR}/laser_config.h.in ${CMAKE_CURRENT_BINARY_DIR}/laser_config.h) PLAYERDRIVER_OPTION (sicklms200 build_sicklms200 ON) -#PLAYERDRIVER_REQUIRE_HEADER (sicklms200 build_sicklms200 linux/serial.h) -PLAYERDRIVER_ADD_DRIVER (sicklms200 build_sicklms200 SOURCES sicklms200.cc) +PLAYERDRIVER_ADD_DRIVER (sicklms200 build_sicklms200 SOURCES sicklms200.cc ${laser_config_h} + INCLUDEDIRS ${CMAKE_CURRENT_BINARY_DIR}) IF (HAVE_STL) PLAYERDRIVER_OPTION (sicklms400 build_sicklms400 ON) @@ -20,7 +20,6 @@ PLAYERDRIVER_ADD_DRIVER (sicklms400 build_sicklms400 SOURCES lms400_cola.cc sicklms400.cc) PLAYERDRIVER_OPTION (sicks3000 build_sicks3000 ON) -#PLAYERDRIVER_REQUIRE_HEADER (sicks3000 build_sicks3000 linux/serial.h) PLAYERDRIVER_ADD_DRIVER (sicks3000 build_sicks3000 SOURCES sicks3000.cc) PLAYERDRIVER_OPTION (laserposeinterpolator build_laserposeinterpolator ON) @@ -36,7 +35,6 @@ PLAYERDRIVER_ADD_DRIVER (lasercutter build_lasercutter SOURCES lasercutter.cc lasertransform.cc) PLAYERDRIVER_OPTION (urglaser build_urglaser ON) -#PLAYERDRIVER_REQUIRE_HEADER (urglaser build_urglaser linux/serial.h) PLAYERDRIVER_ADD_DRIVER (urglaser build_urglaser SOURCES urg_laser.cc urglaserdriver.cc) # Also build and install standalone non-Player lib PLAYER_ADD_LIBRARY (urglaser_standalone urg_laser.cc) Modified: code/player/trunk/server/drivers/laser/sicklms200.cc =================================================================== --- code/player/trunk/server/drivers/laser/sicklms200.cc 2008-08-11 00:22:53 UTC (rev 6949) +++ code/player/trunk/server/drivers/laser/sicklms200.cc 2008-08-12 02:57:00 UTC (rev 6950) @@ -199,9 +199,7 @@ #include <unistd.h> #include <sys/ioctl.h> -// TODO: figure out why this can't be included without using the full path -//#include <laser_config.h> -#include <../drivers/laser/laser_config.h> +#include <laser_config.h> #undef HAVE_HI_SPEED_SERIAL #ifdef HAVE_LINUX_SERIAL_H This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <gb...@us...> - 2008-08-14 17:55:21
|
Revision: 6953 http://playerstage.svn.sourceforge.net/playerstage/?rev=6953&view=rev Author: gbiggs Date: 2008-08-15 00:55:29 +0000 (Fri, 15 Aug 2008) Log Message: ----------- Fixed bug 2051127, added -lrt for Solaris where necessary, fixed some missing includes. Modified Paths: -------------- code/player/trunk/client_libs/libplayerc/CMakeLists.txt code/player/trunk/examples/libplayerc++/CMakeLists.txt code/player/trunk/examples/libplayerc++/CMakeLists.txt.example.in code/player/trunk/libplayertcp/CMakeLists.txt code/player/trunk/server/CMakeLists.txt code/player/trunk/server/drivers/laser/lms400_cola.cc code/player/trunk/server/drivers/laser/sicklms200.cc code/player/trunk/server/drivers/mixed/erratic/erratic.cc code/player/trunk/server/libplayerdrivers/CMakeLists.txt code/player/trunk/utils/playerv/CMakeLists.txt Modified: code/player/trunk/client_libs/libplayerc/CMakeLists.txt =================================================================== --- code/player/trunk/client_libs/libplayerc/CMakeLists.txt 2008-08-14 15:03:10 UTC (rev 6952) +++ code/player/trunk/client_libs/libplayerc/CMakeLists.txt 2008-08-15 00:55:29 UTC (rev 6953) @@ -69,7 +69,7 @@ IF (PLAYER_OS_QNX) TARGET_LINK_LIBRARIES (playerc socket rpc) ELSEIF (PLAYER_OS_SOLARIS) - TARGET_LINK_LIBRARIES (playerc socket nsl) + TARGET_LINK_LIBRARIES (playerc socket nsl rt) ENDIF (PLAYER_OS_QNX) PLAYER_INSTALL_HEADERS (playerc playerc.h) Modified: code/player/trunk/examples/libplayerc++/CMakeLists.txt =================================================================== --- code/player/trunk/examples/libplayerc++/CMakeLists.txt 2008-08-14 15:03:10 UTC (rev 6952) +++ code/player/trunk/examples/libplayerc++/CMakeLists.txt 2008-08-15 00:55:29 UTC (rev 6953) @@ -11,6 +11,9 @@ MACRO (PLAYERCPP_ADD_EXAMPLE _name) ADD_EXECUTABLE (${_name} ${ARGN}) TARGET_LINK_LIBRARIES (${_name} playerc++ playerc playerxdr playererror ${PLAYERC_EXTRA_LINK_LIBRARIES} ${PLAYERCC_EXTRA_LINK_LIBRARIES}) + IF (PLAYER_OS_SOLARIS) + TARGET_LINK_LIBRARIES (${_name} rt) + ENDIF (PLAYER_OS_SOLARIS) ENDMACRO (PLAYERCPP_ADD_EXAMPLE _name) PLAYERCPP_ADD_EXAMPLE (camera camera.cc) Modified: code/player/trunk/examples/libplayerc++/CMakeLists.txt.example.in =================================================================== --- code/player/trunk/examples/libplayerc++/CMakeLists.txt.example.in 2008-08-14 15:03:10 UTC (rev 6952) +++ code/player/trunk/examples/libplayerc++/CMakeLists.txt.example.in 2008-08-15 00:55:29 UTC (rev 6953) @@ -4,6 +4,10 @@ SET (CMAKE_MODULE_PATH @CMAKE_INSTALL_PREFIX@/share/cmake/Modules) INCLUDE (UsePlayerC++) +IF (PLAYER_OS_SOLARIS) + SET (rtLibFlag -lrt) +ENDIF (PLAYER_OS_SOLARIS) + PLAYER_ADD_PLAYERCPP_CLIENT (camera SOURCES camera.cc) PLAYER_ADD_PLAYERCPP_CLIENT (example0 SOURCES example0.cc) PLAYER_ADD_PLAYERCPP_CLIENT (example4 SOURCES example4.cc) @@ -21,10 +25,10 @@ SET (HAVE_BOOST_SIGNALS @USE_BOOST_SIGNALS@) IF (HAVE_BOOST_THREADS OR HAVE_BOOST_SIGNALS) - PLAYER_ADD_PLAYERCPP_CLIENT (example1 SOURCES example1.cc) + PLAYER_ADD_PLAYERCPP_CLIENT (example1 SOURCES example1.cc LINKFLAGS ${rtLibFlag}) PLAYER_ADD_PLAYERCPP_CLIENT (example3 SOURCES example3.cc) - PLAYER_ADD_PLAYERCPP_CLIENT (goto SOURCES goto.cc) - PLAYER_ADD_PLAYERCPP_CLIENT (speech_cpp_client SOURCES speech_cpp_client.cc) + PLAYER_ADD_PLAYERCPP_CLIENT (goto SOURCES goto.cc LINKFLAGS ${rtLibFlag}) + PLAYER_ADD_PLAYERCPP_CLIENT (speech_cpp_client SOURCES speech_cpp_client.cc LINKFLAGS ${rtLibFlag}) ENDIF (HAVE_BOOST_THREADS OR HAVE_BOOST_SIGNALS) IF (HAVE_BOOST_SIGNALS) Modified: code/player/trunk/libplayertcp/CMakeLists.txt =================================================================== --- code/player/trunk/libplayertcp/CMakeLists.txt 2008-08-14 15:03:10 UTC (rev 6952) +++ code/player/trunk/libplayertcp/CMakeLists.txt 2008-08-15 00:55:29 UTC (rev 6953) @@ -34,8 +34,13 @@ PLAYER_ADD_LINK_LIB (z) SET (zLibFlag -lz) ENDIF (HAVE_Z) + IF (PLAYER_OS_SOLARIS) + TARGET_LINK_LIBRARIES (playertcp rt) + PLAYER_ADD_LINK_LIB (rt) + SET (rtLibFlag -lrt) + ENDIF (PLAYER_OS_SOLARIS) PLAYER_MAKE_PKGCONFIG ("playertcp" "Player TCP messaging library - part of the Player Project" - "playererror playercore" "" "" "${zLibFlag} ${SOCKET_LIBS_FLAGS}") + "playererror playercore" "" "" "${zLibFlag} ${rtLibFlag} ${SOCKET_LIBS_FLAGS}") PLAYER_INSTALL_HEADERS (playertcp playertcp.h) ENDIF (INCLUDE_TCP) Modified: code/player/trunk/server/CMakeLists.txt =================================================================== --- code/player/trunk/server/CMakeLists.txt 2008-08-14 15:03:10 UTC (rev 6952) +++ code/player/trunk/server/CMakeLists.txt 2008-08-15 00:55:29 UTC (rev 6953) @@ -24,6 +24,9 @@ IF (HAVE_PLAYERSD) TARGET_LINK_LIBRARIES (player playersd) ENDIF (HAVE_PLAYERSD) +IF (PLAYER_OS_SOLARIS) + TARGET_LINK_LIBRARIES (player rt) +ENDIF (PLAYER_OS_SOLARIS) STRING (REGEX REPLACE "^[ ]+" "" PLAYER_DRIVERSLIB_LINKFLAGS "${PLAYER_DRIVERSLIB_LINKFLAGS}") STRING (REGEX REPLACE "[ ]+$" "" PLAYER_DRIVERSLIB_LINKFLAGS "${PLAYER_DRIVERSLIB_LINKFLAGS}") IF (PLAYER_DRIVERSLIB_LINKFLAGS) Modified: code/player/trunk/server/drivers/laser/lms400_cola.cc =================================================================== --- code/player/trunk/server/drivers/laser/lms400_cola.cc 2008-08-14 15:03:10 UTC (rev 6952) +++ code/player/trunk/server/drivers/laser/lms400_cola.cc 2008-08-15 00:55:29 UTC (rev 6953) @@ -10,6 +10,7 @@ #include "lms400_cola.h" #include <unistd.h> +#include <strings.h> //////////////////////////////////////////////////////////////////////////////// // Constructor. @@ -18,7 +19,7 @@ portno = port; hostname = host; verbose = debug_mode; - bzero (command, BUF_SIZE); + memset (command, 0, BUF_SIZE); MeasurementQueue = new std::vector<MeasurementQueueElement_t >; } @@ -39,12 +40,12 @@ return (-1); // Fill in the sockaddr_in structure values - bzero ((char *) &serv_addr, sizeof (serv_addr)); + memset ((char *) &serv_addr, 0, sizeof (serv_addr)); serv_addr.sin_family = AF_INET; serv_addr.sin_port = htons (portno); - bcopy ((char *)server->h_addr, - (char *)&serv_addr.sin_addr.s_addr, - server->h_length); + memcpy ((char *)&serv_addr.sin_addr.s_addr, + (char *)server->h_addr, + server->h_length); // Attempt to connect if (connect (sockfd, (const sockaddr*)&serv_addr, sizeof (serv_addr)) < 0) @@ -364,7 +365,7 @@ int length = 0; int current = 0; - bzero (buffer, 256); + memset (buffer, 0, 256); if (!MeasurementQueue->empty ()) { if (verbose) printf (">>> Reading from queue...\n"); @@ -501,7 +502,7 @@ int lms400_cola::ReadResult () { - bzero (buffer, 256); + memset (buffer, 0, 256); n = read (sockfd, buffer, 8); if (n < 0) return (-1); Modified: code/player/trunk/server/drivers/laser/sicklms200.cc =================================================================== --- code/player/trunk/server/drivers/laser/sicklms200.cc 2008-08-14 15:03:10 UTC (rev 6952) +++ code/player/trunk/server/drivers/laser/sicklms200.cc 2008-08-15 00:55:29 UTC (rev 6953) @@ -211,7 +211,7 @@ #include <libplayercore/playercore.h> #include <libplayerxdr/playerxdr.h> -//#include <replace/replace.h> +#include <replace/replace.h> extern PlayerTime* GlobalTime; #define DEFAULT_LASER_PORT "/dev/ttyS1" Modified: code/player/trunk/server/drivers/mixed/erratic/erratic.cc =================================================================== --- code/player/trunk/server/drivers/mixed/erratic/erratic.cc 2008-08-14 15:03:10 UTC (rev 6952) +++ code/player/trunk/server/drivers/mixed/erratic/erratic.cc 2008-08-15 00:55:29 UTC (rev 6953) @@ -177,6 +177,7 @@ #include <math.h> #include <termios.h> #include <stdlib.h> // for abs() +#include <sys/stat.h> #include "erratic.h" Modified: code/player/trunk/server/libplayerdrivers/CMakeLists.txt =================================================================== --- code/player/trunk/server/libplayerdrivers/CMakeLists.txt 2008-08-14 15:03:10 UTC (rev 6952) +++ code/player/trunk/server/libplayerdrivers/CMakeLists.txt 2008-08-15 00:55:29 UTC (rev 6953) @@ -45,7 +45,6 @@ ENDIF (NOT HAVE_ROUND) FILTER_DUPLICATES (driversSrcs "${driversSrcs}") -# MESSAGE (STATUS "Sources is ${driversSrcs}") # Set lib-wide link and include directories IF (PLAYER_DRIVERSLIB_INCLUDEDIRS) @@ -65,6 +64,9 @@ IF (HAVE_JPEG) TARGET_LINK_LIBRARIES (playerdrivers playerjpeg) ENDIF (HAVE_JPEG) +IF (PLAYER_OS_SOLARIS) + TARGET_LINK_LIBRARIES (playerdrivers rt) +ENDIF (PLAYER_OS_SOLARIS) # Set C flags on source files that need them FOREACH (driverName ${PLAYER_BUILT_DRIVERS}) Modified: code/player/trunk/utils/playerv/CMakeLists.txt =================================================================== --- code/player/trunk/utils/playerv/CMakeLists.txt 2008-08-14 15:03:10 UTC (rev 6952) +++ code/player/trunk/utils/playerv/CMakeLists.txt 2008-08-15 00:55:29 UTC (rev 6953) @@ -28,6 +28,9 @@ INCLUDE_DIRECTORIES (${PROJECT_SOURCE_DIR}/client_libs ${GTK_INCLUDEDIR}) PLAYER_ADD_EXECUTABLE (playerv ${playervSrcs}) TARGET_LINK_LIBRARIES (playerv playerc playerxdr playererror playerutils rtk ${PLAYERC_EXTRA_LINK_LIBRARIES}) + IF (PLAYER_OS_SOLARIS) + TARGET_LINK_LIBRARIES (playerv rt) + ENDIF (PLAYER_OS_SOLARIS) SET_SOURCE_FILES_PROPERTIES (${playervSrcs} PROPERTIES COMPILE_FLAGS "${GTK_CFLAGS}") SET_TARGET_PROPERTIES (playerv PROPERTIES This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2008-08-16 20:34:17
|
Revision: 6956 http://playerstage.svn.sourceforge.net/playerstage/?rev=6956&view=rev Author: thjc Date: 2008-08-17 03:34:26 +0000 (Sun, 17 Aug 2008) Log Message: ----------- Merged 6930 from 2-1 fix for Cmucam2 which was not matching command messages correctly, and incorrectly acking commands Modified Paths: -------------- code/player/trunk/server/drivers/mixed/cmucam2/cmucam2.cc Property Changed: ---------------- code/player/trunk/ Property changes on: code/player/trunk ___________________________________________________________________ Modified: svn:mergeinfo - /code/player/branches/release-2-1-patches:6672-6673,6738,6834,6886,6905 + /code/player/branches/release-2-1-patches:6672-6673,6738,6834,6886,6905,6930 Modified: code/player/trunk/server/drivers/mixed/cmucam2/cmucam2.cc =================================================================== --- code/player/trunk/server/drivers/mixed/cmucam2/cmucam2.cc 2008-08-16 00:01:56 UTC (rev 6955) +++ code/player/trunk/server/drivers/mixed/cmucam2/cmucam2.cc 2008-08-17 03:34:26 UTC (rev 6956) @@ -311,7 +311,7 @@ assert(hdr); assert(data); - if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, 0, ptz_id)) + if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_PTZ_CMD_STATE, ptz_id)) { player_ptz_cmd_t & command = *reinterpret_cast<player_ptz_cmd_t * > (data); @@ -331,9 +331,6 @@ set_servo_position(fd, 1, -1*tilt_position); } - Publish (this->ptz_id, resp_queue, - PLAYER_MSGTYPE_RESP_ACK, - hdr->subtype); return 0; } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2008-08-16 21:36:42
|
Revision: 6958 http://playerstage.svn.sourceforge.net/playerstage/?rev=6958&view=rev Author: thjc Date: 2008-08-17 04:36:50 +0000 (Sun, 17 Aug 2008) Log Message: ----------- merged 6946 from P2-1 Fixed some obscure timing issues in libplayertcp/remote driver/filewatcher code which only surfaced with specific use of the passthrough driver. Also loop iteration in wait call of filewatcher. Modified Paths: -------------- code/player/trunk/libplayercore/filewatcher.cc code/player/trunk/libplayertcp/playertcp.cc code/player/trunk/libplayertcp/playertcp.h code/player/trunk/libplayertcp/remote_driver.cc code/player/trunk/libplayertcp/remote_driver.h code/player/trunk/server/server.cc Modified: code/player/trunk/libplayercore/filewatcher.cc =================================================================== --- code/player/trunk/libplayercore/filewatcher.cc 2008-08-17 04:11:21 UTC (rev 6957) +++ code/player/trunk/libplayercore/filewatcher.cc 2008-08-17 04:36:50 UTC (rev 6958) @@ -45,6 +45,7 @@ Lock(); if (WatchedFilesArrayCount == 0) { + PLAYER_ERROR("File watcher wait called with no files to watch"); Unlock(); return 0; } @@ -75,20 +76,25 @@ struct timeval t; t.tv_sec = static_cast<int> (floor(Timeout)); t.tv_usec = static_cast<int> ((Timeout - static_cast<int> (floor(Timeout))) * 1e6); - Unlock(); int ret = select (maxfd+1,&ReadFds,&WriteFds,&ExceptFds,&t); if (ret < 0) { PLAYER_ERROR2("Select called failed in File Watcher: %d %s",errno,strerror(errno)); + Unlock(); return ret; } + else if (ret == 0) + { + Unlock(); + return 0; + } int queueless_count = 0; + int match_count = 0; - Lock(); - for (unsigned int ii = 0; ii < WatchedFilesArrayCount && static_cast<int> (ii) < maxfd; ++ii) + for (unsigned int ii = 0; ii < WatchedFilesArrayCount && ret > match_count; ++ii) { int fd = WatchedFiles[ii].fd; QueuePointer &q = WatchedFiles[ii].queue; @@ -98,18 +104,26 @@ (WatchedFiles[ii].Write && FD_ISSET(fd,&WriteFds)) || (WatchedFiles[ii].Except && FD_ISSET(fd,&ExceptFds))) { + match_count++; if (q != NULL) { q->DataAvailable(); } else + { queueless_count++; + } } } } Unlock(); + if (ret != match_count) + { + PLAYER_ERROR1("Failed to match %d file descriptors in select results",ret - match_count); + } + return queueless_count; } @@ -125,32 +139,27 @@ Lock(); // find the first available file descriptor struct fd_driver_pair *next_entry = NULL; - if (WatchedFilesArrayCount < WatchedFilesArraySize) + // first see if there is an empty spot in the list + for (unsigned int ii = 0; ii < WatchedFilesArrayCount; ++ii) { - next_entry = &WatchedFiles[WatchedFilesArrayCount]; - WatchedFilesArrayCount++; + if (WatchedFiles[ii].fd < 0) + { + next_entry = &WatchedFiles[ii]; + break; + } } - else + if (next_entry == NULL) { - // first see if there is an empty spot in the list - for (unsigned int ii = 0; ii < WatchedFilesArrayCount; ++ii) + if (WatchedFilesArrayCount >= WatchedFilesArraySize) { - if (WatchedFiles[ii].fd < 0) - { - next_entry = &WatchedFiles[ii]; - break; - } - } - if (next_entry == NULL) - { // otherwise we allocate some more room for the array size_t orig_size = WatchedFilesArraySize; WatchedFilesArraySize*=2; WatchedFiles = reinterpret_cast<struct fd_driver_pair *> (realloc(WatchedFiles,sizeof(WatchedFiles[0])*WatchedFilesArraySize)); memset(&WatchedFiles[orig_size],0,sizeof(WatchedFiles[0])*(WatchedFilesArraySize-orig_size)); - next_entry = &WatchedFiles[WatchedFilesArrayCount]; - } + next_entry = &WatchedFiles[WatchedFilesArrayCount]; + WatchedFilesArrayCount++; } next_entry->fd = fd; @@ -158,6 +167,7 @@ next_entry->Read = WatchRead; next_entry->Write = WatchWrite; next_entry->Except = WatchExcept; + Unlock(); return 0; } Modified: code/player/trunk/libplayertcp/playertcp.cc =================================================================== --- code/player/trunk/libplayertcp/playertcp.cc 2008-08-17 04:11:21 UTC (rev 6957) +++ code/player/trunk/libplayertcp/playertcp.cc 2008-08-17 04:36:50 UTC (rev 6958) @@ -212,7 +212,9 @@ bool have_lock) { if(!have_lock) + { Lock(); + } unsigned char data[PLAYER_IDENT_STRLEN]; @@ -252,7 +254,6 @@ // set up for later use by global file watcher fileWatcher->AddFileWatch(this->client_ufds[j].fd); - // Create an outgoing queue for this client this->clients[j].queue = QueuePointer(true,PLAYER_MSGQUEUE_DEFAULT_MAXLEN); @@ -327,7 +328,9 @@ } if(!num_accepts) + { return(0); + } for(int i=0; (i<num_listeners) && (num_accepts>0); i++) { @@ -393,9 +396,9 @@ } } free(this->clients[cli].dev_subs); + fileWatcher->RemoveFileWatch(this->clients[cli].fd); if(close(this->clients[cli].fd) < 0) PLAYER_WARN1("close() failed: %s", strerror(errno)); - fileWatcher->RemoveFileWatch(this->clients[cli].fd); this->clients[cli].fd = -1; this->clients[cli].valid = 0; Modified: code/player/trunk/libplayertcp/playertcp.h =================================================================== --- code/player/trunk/libplayertcp/playertcp.h 2008-08-17 04:11:21 UTC (rev 6957) +++ code/player/trunk/libplayertcp/playertcp.h 2008-08-17 04:36:50 UTC (rev 6958) @@ -136,13 +136,13 @@ /** Total size of @p decode_readbuffer */ int decode_readbuffersize; + void Lock(); + void Unlock(); public: PlayerTCP(); ~PlayerTCP(); - void Lock(); - void Unlock(); static void InitGlobals(void); @@ -150,7 +150,7 @@ int Listen(int* ports, int num_ports, int* new_ports=NULL); int Listen(int port); - QueuePointer AddClient(struct sockaddr_in* cliaddr, + QueuePointer AddClient(struct sockaddr_in* cliaddr, unsigned int local_host, unsigned int local_port, int newsock, Modified: code/player/trunk/libplayertcp/remote_driver.cc =================================================================== --- code/player/trunk/libplayertcp/remote_driver.cc 2008-08-17 04:11:21 UTC (rev 6957) +++ code/player/trunk/libplayertcp/remote_driver.cc 2008-08-17 04:36:50 UTC (rev 6958) @@ -2,7 +2,6 @@ * Player - One Hell of a Robot Server * Copyright (C) <insert dates here> * <insert author's name(s) here> - * * * 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 @@ -53,24 +52,20 @@ #include <libplayerxdr/playerxdr.h> #include "remote_driver.h" -TCPRemoteDriver::TCPRemoteDriver(player_devaddr_t addr, void* arg) - : Driver(NULL, 0, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN) +TCPRemoteDriver::TCPRemoteDriver(player_devaddr_t addr, void* arg) + : Driver(NULL, 0, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN), + sock(-1), + setup_timeout (DEFAULT_SETUP_TIMEOUT) { - if(arg) - this->ptcp = (PlayerTCP*)arg; - else - this->ptcp = NULL; - - this->sock = -1; - this->setup_timeout = DEFAULT_SETUP_TIMEOUT; + ptcp = reinterpret_cast<PlayerTCP*> (arg); } TCPRemoteDriver::~TCPRemoteDriver() { } -int -TCPRemoteDriver::Setup() +int +TCPRemoteDriver::Setup() { struct sockaddr_in server; char banner[PLAYER_IDENT_STRLEN]; @@ -92,7 +87,7 @@ } - // Construct socket + // Construct socket this->sock = socket(PF_INET, SOCK_STREAM, 0); if(this->sock < 0) { @@ -104,17 +99,17 @@ server.sin_addr.s_addr = this->device_addr.host; server.sin_port = htons(this->device_addr.robot); - // Connect the socket + // Connect the socket if(connect(this->sock, (struct sockaddr*)&server, sizeof(server)) < 0) { PLAYER_ERROR3("connect call on [%s:%u] failed with error [%s]", this->ipaddr, - this->device_addr.robot, + this->device_addr.robot, strerror(errno)); return(-1); } - printf("connected to: %s:%u\n", + PLAYER_MSG2(2,"connected to: %s:%u\n", this->ipaddr, this->device_addr.robot); // make the socket non-blocking @@ -141,7 +136,7 @@ numread=0; while(numread < (int)sizeof(banner)) { - if((thisnumread = read(this->sock, banner+numread, + if((thisnumread = read(this->sock, banner+numread, sizeof(banner)-numread)) < 0) { if(errno != EAGAIN) @@ -169,13 +164,14 @@ // Add this socket for monitoring this->kill_flag = 0; - this->queue = this->ptcp->AddClient(NULL, - this->device_addr.host, - this->device_addr.robot, - this->sock, + this->queue = this->ptcp->AddClient(NULL, + this->device_addr.host, + this->device_addr.robot, + this->sock, false, &this->kill_flag, (this->ptcp->thread == pthread_self())); + PLAYER_MSG0(5,"Adding new TCPRemoteDriver to the PlayerTCP Client List...Success"); return(0); @@ -204,7 +200,7 @@ req.driver_name_count = 0; // Encode the body - if((encode_msglen = + if((encode_msglen = player_device_req_pack(buf + PLAYERXDR_MSGHDR_SIZE, sizeof(buf)-PLAYERXDR_MSGHDR_SIZE, &req, PLAYERXDR_ENCODE)) < 0) @@ -231,7 +227,7 @@ while(numbytes < encode_msglen) { - if((thisnumbytes = write(this->sock, buf+numbytes, + if((thisnumbytes = write(this->sock, buf+numbytes, encode_msglen-numbytes)) < 0) { if(errno != EAGAIN) @@ -256,13 +252,13 @@ // In any case, explicitly unsubscribing is just a courtesy. if(mode == PLAYER_CLOSE_MODE) return(0); - + // Receive the response header GlobalTime->GetTimeDouble(&t1); numbytes = 0; while(numbytes < PLAYERXDR_MSGHDR_SIZE) { - if((thisnumbytes = read(this->sock, buf+numbytes, + if((thisnumbytes = read(this->sock, buf+numbytes, PLAYERXDR_MSGHDR_SIZE-numbytes)) < 0) { if(errno != EAGAIN) @@ -289,7 +285,7 @@ } // Is it the right kind of message? - if(!Message::MatchMessage(&hdr, + if(!Message::MatchMessage(&hdr, PLAYER_MSGTYPE_RESP_ACK, PLAYER_PLAYER_REQ_DEV, hdr.addr)) @@ -303,7 +299,7 @@ numbytes = 0; while(numbytes < (int)hdr.size) { - if((thisnumbytes = read(this->sock, buf+PLAYERXDR_MSGHDR_SIZE+numbytes, + if((thisnumbytes = read(this->sock, buf+PLAYERXDR_MSGHDR_SIZE+numbytes, hdr.size-numbytes)) < 0) { if(errno != EAGAIN) @@ -349,8 +345,8 @@ return(0); } -int -TCPRemoteDriver::Shutdown() +int +TCPRemoteDriver::Shutdown() { // Have we already been killed? if(!this->kill_flag) @@ -364,27 +360,12 @@ this->ptcp->DeleteClient(this->queue, (this->ptcp->thread == pthread_self())); } - return(0); + return(0); } -void -TCPRemoteDriver::Update() -{ - if(this->ptcp->thread == pthread_self()) - { - //this->ptcp->Read(0,true); - this->ptcp->Lock(); - this->ptcp->ReadClient(this->queue); - this->ptcp->Unlock(); - } - this->ProcessMessages(); - if(this->ptcp->thread == pthread_self()) - this->ptcp->Write(false); -} - -int -TCPRemoteDriver::ProcessMessage(QueuePointer &resp_queue, - player_msghdr * hdr, +int +TCPRemoteDriver::ProcessMessage(QueuePointer &resp_queue, + player_msghdr * hdr, void * data) { // Is it data from the remote device? @@ -442,7 +423,7 @@ } } // Forward response (success or failure) from the laser - else if((Message::MatchMessage(hdr, PLAYER_MSGTYPE_RESP_ACK, + else if((Message::MatchMessage(hdr, PLAYER_MSGTYPE_RESP_ACK, -1, this->device_addr)) || (Message::MatchMessage(hdr, PLAYER_MSGTYPE_RESP_NACK, -1, this->device_addr))) @@ -459,7 +440,7 @@ } -Driver* +Driver* TCPRemoteDriver::TCPRemoteDriver_Init(player_devaddr_t addr, void* arg) { return((Driver*)(new TCPRemoteDriver(addr, arg))); Modified: code/player/trunk/libplayertcp/remote_driver.h =================================================================== --- code/player/trunk/libplayertcp/remote_driver.h 2008-08-17 04:11:21 UTC (rev 6957) +++ code/player/trunk/libplayertcp/remote_driver.h 2008-08-17 04:36:50 UTC (rev 6958) @@ -2,8 +2,8 @@ * Player - One Hell of a Robot Server * Copyright (C) <insert dates here> * <insert author's name(s) here> - * * + * * 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 @@ -63,9 +63,8 @@ virtual int Setup(); virtual int Shutdown(); - virtual void Update(); - virtual int ProcessMessage(QueuePointer & resp_queue, - player_msghdr * hdr, + virtual int ProcessMessage(QueuePointer & resp_queue, + player_msghdr * hdr, void * data); static Driver* TCPRemoteDriver_Init(player_devaddr_t addr, void* arg); Modified: code/player/trunk/server/server.cc =================================================================== --- code/player/trunk/server/server.cc 2008-08-17 04:11:21 UTC (rev 6957) +++ code/player/trunk/server/server.cc 2008-08-17 04:36:50 UTC (rev 6958) @@ -294,26 +294,27 @@ while(!player_quit) { // wait until something other than driver requested watches happens - fileWatcher->Wait(); - - if(ptcp->Accept(0) < 0) + int numready = fileWatcher->Wait(0.01); // run at a minimum of 100Hz for other drivers + if (numready > 0) { - PLAYER_ERROR("failed while accepting new TCP connections"); - break; - } + if(ptcp->Accept(0) < 0) + { + PLAYER_ERROR("failed while accepting new TCP connections"); + break; + } - if(ptcp->Read(100,false) < 0) - { - PLAYER_ERROR("failed while reading from TCP clients"); - break; - } + if(ptcp->Read(0,false) < 0) + { + PLAYER_ERROR("failed while reading from TCP clients"); + break; + } - if(pudp->Read(0) < 0) - { - PLAYER_ERROR("failed while reading from UDP clients"); - break; + if(pudp->Read(0) < 0) + { + PLAYER_ERROR("failed while reading from UDP clients"); + break; + } } - deviceTable->UpdateDevices(); if(ptcp->Write(false) < 0) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <gb...@us...> - 2008-08-24 17:58:26
|
Revision: 6977 http://playerstage.svn.sourceforge.net/playerstage/?rev=6977&view=rev Author: gbiggs Date: 2008-08-25 00:58:33 +0000 (Mon, 25 Aug 2008) Log Message: ----------- Applied patch 2062373 Modified Paths: -------------- code/player/trunk/client_libs/libplayerc/dev_ranger.c code/player/trunk/client_libs/libplayerc++/actarrayproxy.cc code/player/trunk/client_libs/libplayerc++/audioproxy.cc code/player/trunk/client_libs/libplayerc++/cameraproxy.cc code/player/trunk/client_libs/libplayerc++/limbproxy.cc code/player/trunk/cmake/internal/FindOS.cmake code/player/trunk/cmake/internal/SearchForStuff.cmake code/player/trunk/server/drivers/blackboard/localbb/localbb.cpp code/player/trunk/server/drivers/mixed/mricp/src/Timer.cpp code/player/trunk/server/drivers/mixed/mricp/src/mricp_driver.cpp code/player/trunk/server/drivers/wsn/accel_calib.cc code/player/trunk/server/drivers/wsn/mica2.cc code/player/trunk/utils/logsplitter/logsplitter.c code/player/trunk/utils/playercam/playercam.c code/player/trunk/utils/playerv/pv_dev_actarray.c code/player/trunk/utils/playerv/pv_dev_gripper.c code/player/trunk/utils/playerv/pv_dev_vectormap.c Modified: code/player/trunk/client_libs/libplayerc/dev_ranger.c =================================================================== --- code/player/trunk/client_libs/libplayerc/dev_ranger.c 2008-08-22 18:19:44 UTC (rev 6976) +++ code/player/trunk/client_libs/libplayerc/dev_ranger.c 2008-08-25 00:58:33 UTC (rev 6977) @@ -52,7 +52,7 @@ { playerc_ranger_t *device; - device = malloc(sizeof(playerc_ranger_t)); + device = (playerc_ranger_t*) malloc(sizeof(playerc_ranger_t)); memset(device, 0, sizeof(playerc_ranger_t)); playerc_device_init(&device->info, client, PLAYER_RANGER_CODE, index, (playerc_putmsg_fn_t) playerc_ranger_putmsg); @@ -98,6 +98,9 @@ // Calculate bearings void playerc_ranger_calculate_bearings(playerc_ranger_t *device) { + double b; + uint32_t ii; + device->bearings_count = device->ranges_count; if (device->bearings_count == 0 && device->bearings != NULL) { @@ -113,8 +116,7 @@ return; } - double b = device->min_angle; - uint32_t ii; + b = device->min_angle; for (ii = 0; ii < device->bearings_count; ii++) { device->bearings[ii] = b; @@ -127,6 +129,9 @@ // Calculate scan points void playerc_ranger_calculate_points(playerc_ranger_t *device) { + double b; + uint32_t ii; + device->points_count = device->ranges_count; if (device->points_count == 0 && device->points != NULL) { @@ -142,8 +147,7 @@ return; } - double b = device->min_angle; - uint32_t ii; + b = device->min_angle; for (ii = 0; ii < device->points_count; ii++) { double r = device->ranges[ii]; Modified: code/player/trunk/client_libs/libplayerc++/actarrayproxy.cc =================================================================== --- code/player/trunk/client_libs/libplayerc++/actarrayproxy.cc 2008-08-22 18:19:44 UTC (rev 6976) +++ code/player/trunk/client_libs/libplayerc++/actarrayproxy.cc 2008-08-25 00:58:33 UTC (rev 6977) @@ -48,6 +48,7 @@ #include <sstream> #include <iomanip> #include <vector> +#include <iostream> #include "playerc++.h" #include "debug.h" @@ -94,7 +95,7 @@ player_actarray_actuatorgeom_t geom; int old_precision = os.precision(3); - std::_Ios_Fmtflags old_flags = os.flags(); + std::ios::fmtflags old_flags = os.flags(); os.setf(std::ios::fixed); os << a.GetCount () << " actuators:" << std::endl; Modified: code/player/trunk/client_libs/libplayerc++/audioproxy.cc =================================================================== --- code/player/trunk/client_libs/libplayerc++/audioproxy.cc 2008-08-22 18:19:44 UTC (rev 6976) +++ code/player/trunk/client_libs/libplayerc++/audioproxy.cc 2008-08-25 00:58:33 UTC (rev 6977) @@ -47,6 +47,7 @@ #include <cassert> #include <sstream> #include <iomanip> +#include <iostream> #include "playerc++.h" #include "debug.h" @@ -93,7 +94,7 @@ player_audio_mixer_channel_t channel; int old_precision = os.precision(3); - std::_Ios_Fmtflags old_flags = os.flags(); + std::ios::fmtflags old_flags = os.flags(); os.setf(std::ios::fixed); int NumChannelDetails = a.GetMixerDetailsCount(); Modified: code/player/trunk/client_libs/libplayerc++/cameraproxy.cc =================================================================== --- code/player/trunk/client_libs/libplayerc++/cameraproxy.cc 2008-08-22 18:19:44 UTC (rev 6976) +++ code/player/trunk/client_libs/libplayerc++/cameraproxy.cc 2008-08-25 00:58:33 UTC (rev 6977) @@ -46,6 +46,9 @@ #include <cassert> #include <sstream> #include <iomanip> +#if __GNUC__ > 2 + #include <locale> +#endif #include "playerc++.h" #include "debug.h" @@ -95,7 +98,9 @@ CameraProxy::SaveFrame(const std::string aPrefix, uint32_t aWidth) { std::ostringstream filename; +#if __GNUC__ > 2 filename.imbue(std::locale("")); +#endif filename.fill('0'); filename << aPrefix << std::setw(aWidth) << mFrameNo++; Modified: code/player/trunk/client_libs/libplayerc++/limbproxy.cc =================================================================== --- code/player/trunk/client_libs/libplayerc++/limbproxy.cc 2008-08-22 18:19:44 UTC (rev 6976) +++ code/player/trunk/client_libs/libplayerc++/limbproxy.cc 2008-08-25 00:58:33 UTC (rev 6977) @@ -47,6 +47,7 @@ #include <cassert> #include <sstream> #include <iomanip> +#include <iostream> #include "playerc++.h" #include "debug.h" @@ -94,7 +95,7 @@ player_limb_geom_req_t geom = a.GetGeom (); int old_precision = os.precision(3); - std::_Ios_Fmtflags old_flags = os.flags(); + std::ios::fmtflags old_flags = os.flags(); os.setf(std::ios::fixed); os << "Limb offset: " << geom.basePos.px << ", " << geom.basePos.py << ", " << geom.basePos.pz << endl; Modified: code/player/trunk/cmake/internal/FindOS.cmake =================================================================== --- code/player/trunk/cmake/internal/FindOS.cmake 2008-08-22 18:19:44 UTC (rev 6976) +++ code/player/trunk/cmake/internal/FindOS.cmake 2008-08-25 00:58:33 UTC (rev 6977) @@ -5,7 +5,7 @@ # Nor *BSD STRING (REGEX MATCH "BSD" PLAYER_OS_BSD ${CMAKE_SYSTEM_NAME}) # Or Solaris. I'm seeing a trend, here -STRING (REGEX MATCH "Solaris" PLAYER_OS_SOLARIS ${CMAKE_SYSTEM_NAME}) +STRING (REGEX MATCH "SunOS" PLAYER_OS_SOLARIS ${CMAKE_SYSTEM_NAME}) # Windows is easy (for once) IF (WIN32) Modified: code/player/trunk/cmake/internal/SearchForStuff.cmake =================================================================== --- code/player/trunk/cmake/internal/SearchForStuff.cmake 2008-08-22 18:19:44 UTC (rev 6976) +++ code/player/trunk/cmake/internal/SearchForStuff.cmake 2008-08-25 00:58:33 UTC (rev 6977) @@ -37,6 +37,7 @@ CHECK_INCLUDE_FILES (stdint.h HAVE_STDINT_H) CHECK_INCLUDE_FILES (strings.h HAVE_STRINGS_H) CHECK_INCLUDE_FILES (dns_sd.h HAVE_DNS_SD) +CHECK_INCLUDE_FILES (sys/filio.h HAVE_SYS_FILIO_H) IF (HAVE_DNS_SD) CHECK_LIBRARY_EXISTS (dns_sd DNSServiceRefDeallocate "${PLAYER_EXTRA_LIB_DIRS}" HAVE_DNS_SD) ENDIF (HAVE_DNS_SD) Modified: code/player/trunk/server/drivers/blackboard/localbb/localbb.cpp =================================================================== --- code/player/trunk/server/drivers/blackboard/localbb/localbb.cpp 2008-08-22 18:19:44 UTC (rev 6976) +++ code/player/trunk/server/drivers/blackboard/localbb/localbb.cpp 2008-08-25 00:58:33 UTC (rev 6977) @@ -66,13 +66,15 @@ /** @} */ #include <sys/types.h> // required by Darwin -#include <stdlib.h> #include <libplayercore/playercore.h> #include <libplayercore/error.h> #include <vector> #include <map> -#include <strings.h> #include <iostream> +#include <cstdlib> +#include <cstddef> +#include <string> +#include <cstring> bool operator ==(const player_devaddr_t &a, const player_devaddr_t &b); bool operator <(const player_devaddr_t &a, const player_devaddr_t &b); @@ -108,9 +110,9 @@ BlackBoardEntry() { key = ""; group = ""; } /** Entry label */ - string key; + std::string key; /** Secondary identifier. */ - string group; + std::string group; /** Entry data */ EntryData data; } BlackBoardEntry; @@ -154,8 +156,8 @@ BlackBoardEntry result; result.data.type = entry.type; result.data.subtype = entry.subtype; - result.key = string(entry.key); - result.group = string(entry.group); + result.key = std::string(entry.key); + result.group = std::string(entry.group); result.data.data_count = entry.data_count; result.data.data = new uint8_t[result.data.data_count]; memcpy(result.data.data, entry.data, result.data.data_count); @@ -258,26 +260,26 @@ * @param group Second identifier. * @param resp_queue Player response queue of the subscriber. */ - BlackBoardEntry SubscribeKey(const string &key, const string &group, const QueuePointer &resp_queue, const player_devaddr_t addr); + BlackBoardEntry SubscribeKey(const std::string &key, const std::string &group, const QueuePointer &resp_queue, const player_devaddr_t addr); /** @brief Remove the key and queue combination from the listeners hash-map. * @param key Entry key. * @param group Second identifier. * @param qp Player response queue of the unsubscriber. * @return Blackboard entry containing the value of the key and group. */ - void UnsubscribeKey(const string &key, const string &group, const QueuePointer &qp); + void UnsubscribeKey(const std::string &key, const std::string &group, const QueuePointer &qp); /** @brief Add a group to the group listeners hash-map and return all entries of that group * @param group Entry gruop * @param qp resp_queue Player response queue of the subscriber * @return Vector of blackboard entries of that group */ - vector<BlackBoardEntry> SubscribeGroup(const string &group, const QueuePointer &qp, const player_devaddr_t addr); + vector<BlackBoardEntry> SubscribeGroup(const std::string &group, const QueuePointer &qp, const player_devaddr_t addr); /** * @brief Remove the group from the group listeners hash-map. * @param group Entry group * @param qp Player response queue of the unsubscriber */ - void UnsubscribeGroup(const string &group, const QueuePointer &qp); + void UnsubscribeGroup(const std::string &group, const QueuePointer &qp); /** @brief Set the entry in the entries hashmap. * * @param entry BlackBoardEntry that must be put in the hashmap. @@ -295,16 +297,16 @@ * map<group, map<key, entry> > */ - map<string, map<string, BlackBoardEntry> > entries; + map<std::string, map<std::string, BlackBoardEntry> > entries; /** Map of labels to listening queues. * map<group, map<key, vector<device queue> > > */ - map<string, map<string, vector<QueuePointer> > > listeners; + map<std::string, map<std::string, vector<QueuePointer> > > listeners; /** Map of groups to queues subscribed to groups. * map<group, vector<device queue> > */ - map<string, vector<QueuePointer> > group_listeners; + map<std::string, vector<QueuePointer> > group_listeners; /** Map of queues to devices. Used to remove unneeded queues when a device is unsubscribed. */ map<player_devaddr_t, QueuePointer> subscribed_devices; @@ -314,10 +316,10 @@ int LocalBB::Unsubscribe(player_devaddr_t addr) { QueuePointer &qp = subscribed_devices[addr]; - for (map<string, map<string, vector<QueuePointer> > >::iterator itr = listeners.begin(); itr != listeners.end(); itr++) + for (map<std::string, map<std::string, vector<QueuePointer> > >::iterator itr = listeners.begin(); itr != listeners.end(); itr++) { - map<string, vector<QueuePointer> > &keys = (*itr).second; - for (map<string, vector<QueuePointer> >::iterator jtr = keys.begin(); jtr != keys.end(); jtr++) + map<std::string, vector<QueuePointer> > &keys = (*itr).second; + for (map<std::string, vector<QueuePointer> >::iterator jtr = keys.begin(); jtr != keys.end(); jtr++) { vector<QueuePointer> &qps = (*jtr).second; vector<vector<QueuePointer>::iterator> remove_list; @@ -335,7 +337,7 @@ } } - for (map<string, vector<QueuePointer> >::iterator itr = group_listeners.begin(); itr != group_listeners.end(); itr++) + for (map<std::string, vector<QueuePointer> >::iterator itr = group_listeners.begin(); itr != group_listeners.end(); itr++) { vector<QueuePointer> &qps = (*itr).second; vector<vector<QueuePointer>::iterator> remove_list; @@ -497,7 +499,7 @@ // Retrieve the entry for the key player_blackboard_entry_t *request = reinterpret_cast<player_blackboard_entry_t*>(data); - BlackBoardEntry current_value = entries[string(request->group)][string(request->key)]; + BlackBoardEntry current_value = entries[std::string(request->group)][std::string(request->key)]; // Convert the entry player_blackboard_entry_t response = ToPlayerBlackBoardEntry(current_value); @@ -685,7 +687,7 @@ //////////////////////////////////////////////////////////////////////////////// // Add a device to the listener list for a key. Return the current value of the entry. -BlackBoardEntry LocalBB::SubscribeKey(const string &key, const string &group, const QueuePointer &resp_queue, const player_devaddr_t addr) +BlackBoardEntry LocalBB::SubscribeKey(const std::string &key, const std::string &group, const QueuePointer &resp_queue, const player_devaddr_t addr) { listeners[group][key].push_back(resp_queue); subscribed_devices[addr] = resp_queue; @@ -700,7 +702,7 @@ //////////////////////////////////////////////////////////////////////////////// // Remove a device from the listener list for a key. -void LocalBB::UnsubscribeKey(const string &key, const string &group, const QueuePointer &qp) +void LocalBB::UnsubscribeKey(const std::string &key, const std::string &group, const QueuePointer &qp) { vector<QueuePointer> &devices = listeners[group][key]; @@ -716,16 +718,16 @@ //////////////////////////////////////////////////////////////////////////////// // Add a device to the group listener map. Return vector of entries for that group. -vector<BlackBoardEntry> LocalBB::SubscribeGroup(const string &group, const QueuePointer &qp, const player_devaddr_t addr) +vector<BlackBoardEntry> LocalBB::SubscribeGroup(const std::string &group, const QueuePointer &qp, const player_devaddr_t addr) { group_listeners[group].push_back(qp); subscribed_devices[addr] = qp; vector<BlackBoardEntry> group_entries; //map<group, map<key, entry> > - map<string, BlackBoardEntry> &entry_map = entries[group]; + map<std::string, BlackBoardEntry> &entry_map = entries[group]; - for (map<string, BlackBoardEntry>::iterator itr = entry_map.begin(); itr != entry_map.end(); itr++) + for (map<std::string, BlackBoardEntry>::iterator itr = entry_map.begin(); itr != entry_map.end(); itr++) { BlackBoardEntry current_value = (*itr).second; if (current_value.key == "") @@ -743,7 +745,7 @@ //////////////////////////////////////////////////////////////////////////////// // Remove a device from the group listener map -void LocalBB::UnsubscribeGroup(const string &group, const QueuePointer &qp) +void LocalBB::UnsubscribeGroup(const std::string &group, const QueuePointer &qp) { vector<QueuePointer> &devices = group_listeners[group]; Modified: code/player/trunk/server/drivers/mixed/mricp/src/Timer.cpp =================================================================== --- code/player/trunk/server/drivers/mixed/mricp/src/Timer.cpp 2008-08-22 18:19:44 UTC (rev 6976) +++ code/player/trunk/server/drivers/mixed/mricp/src/Timer.cpp 2008-08-25 00:58:33 UTC (rev 6977) @@ -1,4 +1,6 @@ #include "Timer.h" +#include <ctime> +#include <cstddef> Timer::Timer() { @@ -7,8 +9,8 @@ double Timer::TimeElapsed() // return in usec { gettimeofday(&end_time,NULL); - time_diff = ((double) end_time.tv_sec*1e6 + (double)end_time.tv_usec) - - ((double) start_time.tv_sec*1e6 + (double)start_time.tv_usec); + time_diff = ((double) end_time.tv_sec*1000000 + (double)end_time.tv_usec) - + ((double) start_time.tv_sec*1000000 + (double)start_time.tv_usec); return time_diff; } Timer::~Timer() @@ -20,7 +22,17 @@ } void Timer::Synch(double period) { + struct timespec ts; + int us; + double time_elapsed = this->TimeElapsed(); - if( time_elapsed < period*1e3) - usleep((int)(period*1e3 -time_elapsed)); + if( time_elapsed < period*1000) + usleep((int)(period*1000 -time_elapsed)); + if (time_elapsed < (period*1000)) + { + us = static_cast<int>(period*1000-time_elapsed); + ts.tv_sec = us/1000000; + ts.tv_nsec = (us%1000000)*1000; + nanosleep(&ts, NULL); + } } Modified: code/player/trunk/server/drivers/mixed/mricp/src/mricp_driver.cpp =================================================================== --- code/player/trunk/server/drivers/mixed/mricp/src/mricp_driver.cpp 2008-08-22 18:19:44 UTC (rev 6976) +++ code/player/trunk/server/drivers/mixed/mricp/src/mricp_driver.cpp 2008-08-25 00:58:33 UTC (rev 6977) @@ -36,22 +36,24 @@ #include <sys/time.h> #include <fcntl.h> #include <termios.h> -#include <stdio.h> -#include <strings.h> +#include <cstdio> #include <unistd.h> -#include <stdlib.h> -#include <errno.h> -#include <string.h> +#include <cstdlib> +#include <cstddef> +#include <cerrno> +#include <cstring> +#include <string> #include <pthread.h> -#include <math.h> +#include <cmath> #include <fstream> #include <iostream> -#include <signal.h> +#include <csignal> +#include <vector> #include <netinet/in.h> // Player includes #include <libplayercore/playercore.h> -#include <assert.h> +#include <cassert> #include "icp.h" #include "map.h" #include "lasermodel.h" Modified: code/player/trunk/server/drivers/wsn/accel_calib.cc =================================================================== --- code/player/trunk/server/drivers/wsn/accel_calib.cc 2008-08-22 18:19:44 UTC (rev 6976) +++ code/player/trunk/server/drivers/wsn/accel_calib.cc 2008-08-25 00:58:33 UTC (rev 6977) @@ -383,13 +383,11 @@ NodeCalibrationValues Accel_Calib::FindNodeValues (unsigned int nodeID) { NodeCalibrationValues n; + NCV::iterator it; - unsigned int i = 0; - - for (i = 0; i < ncv.size (); i++) + for (it = ncv.begin (); it != ncv.end (); it++) { - n = ncv.at (i); - + n = *it; if (n.node_id == nodeID) break; } Modified: code/player/trunk/server/drivers/wsn/mica2.cc =================================================================== --- code/player/trunk/server/drivers/wsn/mica2.cc 2008-08-22 18:19:44 UTC (rev 6976) +++ code/player/trunk/server/drivers/wsn/mica2.cc 2008-08-25 00:58:33 UTC (rev 6977) @@ -883,15 +883,13 @@ NodeCalibrationValues Mica2::FindNodeValues (unsigned int nodeID) { NodeCalibrationValues n; + NCV::iterator it; - unsigned int i = 0; - - for (i = 0; i < ncv.size (); i++) + for (it = ncv.begin (); it != ncv.end (); it++) { - n = ncv.at (i); - - if (n.node_id == nodeID) - break; + n = *it; + if (n.node_id == nodeID) + break; } return n; Modified: code/player/trunk/utils/logsplitter/logsplitter.c =================================================================== --- code/player/trunk/utils/logsplitter/logsplitter.c 2008-08-22 18:19:44 UTC (rev 6976) +++ code/player/trunk/utils/logsplitter/logsplitter.c 2008-08-25 00:58:33 UTC (rev 6977) @@ -19,8 +19,8 @@ * CVS: $Id$ */ /* - Desc : Splits log files into smaller chunks, based on the difference between - consecutive timestamp entries. A bit ugly, but it works. There's room + Desc : Splits log files into smaller chunks, based on the difference between + consecutive timestamp entries. A bit ugly, but it works. There's room for optimization, so feel free. Author: Radu Bogdan Rusu Date : 20th of September, 2006 @@ -35,7 +35,7 @@ #include <sys/stat.h> // Splits a logfile and returns the new file handle -FILE +FILE *copySplitData (FILE *input, double t2, long before) { char data[1024]; @@ -48,10 +48,10 @@ // Save current position in file long currentPos = ftell (input); - + // Seek to the beginning of the file fseek (input, 0L, SEEK_SET); - + // Read the first entry while (!feof (input)) { @@ -75,7 +75,7 @@ { output = fopen (fileName, "w+"); printf ("I: Creating... %s\n", fileName); - + // Copy the relevant data while (!feof (input)) { @@ -84,10 +84,10 @@ break; fputs (data, output); } - + // Seek back in the source file fseek (input, currentPos - strlen (data), SEEK_SET); - + // Close output file fclose (output); } @@ -97,18 +97,18 @@ // Seek back in the source file fseek (input, currentPos - before, SEEK_SET); } - + // Create a new file and copy the remainings there sprintf (fileName, "%lf-split.log", t2); if (stat (fileName, &fbuf) != 0) { rest = fopen (fileName, "w+"); - printf ("I: Creating... %s\n", fileName); + printf ("I: Creating... %s\n", fileName); while (1) { fgets (data, 1024, input); lastdatalen = strlen (data); - if (feof (input)) + if (feof (input)) break; fputs (data, rest); } @@ -118,7 +118,7 @@ // Truncate the remainings from the source file ftruncate (fileno (input), currentPos - before); fclose (input); - + return rest; } else @@ -141,7 +141,7 @@ float min_timedifference; struct stat fbuf; struct stat ftempbuf; - + // We need 2 parameters if (argc != 3) { @@ -150,13 +150,13 @@ "USAGE: logsplitter [min_time_difference_in_seconds] [FILE]\n\n"); return -1; } - + // Get the minimum time difference between two consecutive timestamps min_timedifference = atof (argv[1]); base_filename = argv[2]; - + printf ("I: Minimum time difference is: %f seconds.\n", min_timedifference); - + // Open file for reading fd = fopen (base_filename, "r+"); if (!fd) @@ -175,17 +175,17 @@ printf ("E: Cannot create a temporary file! Aborting...\n"); return -1; } - + // Copy the content of our logfile to that temporary file while (1) { fgets (buf, 1024, fd); - if (feof (fd)) + if (feof (fd)) break; fputs (buf, tempfd); } fflush (tempfd); - + // Close the original logfile fclose (fd); @@ -196,7 +196,7 @@ printf ("E: The temporary file differs than the original log file by %ld bytes! Aborting...", fbuf.st_size - ftempbuf.st_size); } - + // Get an initial reading rewind (tempfd); while (!feof (tempfd)) @@ -223,13 +223,17 @@ sscanf (buf, "%lf", &t2); after = ftell (tempfd); - + // Verify if we need a break if ((fabs (t2 - t1) > min_timedifference) && (after - before > 0)) { - time_t t = (time_t)t1; - ctime_r (&t, btime); - printf ("I: Break (%f) needed after T = %f -> %s", t2 - t1, t1, btime); + time_t t = (time_t)t1; +#if defined (__SVR4) && defined (__sun) + ctime_r (&t, btime, sizeof (btime)); +#else + ctime_r (&t, btime); +#endif + printf ("I: Break (%f) needed after T = %f -> %s", t2 - t1, t1, btime); tempfd = copySplitData (tempfd, t2, after - before); if (ftell (tempfd) == 0) @@ -241,6 +245,6 @@ // Close file fclose (tempfd); - + return 0; } Modified: code/player/trunk/utils/playercam/playercam.c =================================================================== --- code/player/trunk/utils/playercam/playercam.c 2008-08-22 18:19:44 UTC (rev 6976) +++ code/player/trunk/utils/playercam/playercam.c 2008-08-25 00:58:33 UTC (rev 6977) @@ -190,9 +190,10 @@ GdkPixbuf *blobbuf= NULL; GdkGC *gc = NULL; GtkWidget *drawing_area = GTK_WIDGET(data); - gc = GTK_WIDGET(drawing_area)->style->fg_gc[GTK_WIDGET_STATE(GTK_WIDGET(drawing_area))]; uint16_t i; + gc = GTK_WIDGET(drawing_area)->style->fg_gc[GTK_WIDGET_STATE(GTK_WIDGET(drawing_area))]; + player_update(); if (g_blob_count > 0) @@ -447,7 +448,7 @@ assert(g_width>0); assert(g_height>0); - playerc_client_datamode(g_client,PLAYER_DATAMODE_PULL); + playerc_client_datamode(g_client,PLAYER_DATAMODE_PULL); playerc_client_set_replace_rule(g_client,-1,-1,PLAYER_MSGTYPE_DATA,-1,1); } @@ -462,17 +463,17 @@ { // Decompress the image if necessary playerc_camera_decompress(g_camera); + assert(allocated_size > g_camera->image_count*3); // figure out the colorspace switch (g_camera->format) { - assert(allocated_size > g_camera->image_count*3); case PLAYER_CAMERA_FORMAT_MONO8: // we should try to use the alpha layer, // but for now we need to change // the image data for (i=0;i<g_camera->image_count;++i) { - + memcpy(g_img+i*3, g_camera->image+i, 3); } break; @@ -482,8 +483,8 @@ // Transform to MONO8 for (i = 0; i < g_camera->image_count; i++, j+=2) { - g_img[i*3+1] = g_img[i*3+2] = g_img[i*3+3] = - ((unsigned char)(g_camera->image[j]) << 8) + + g_img[i*3+1] = g_img[i*3+2] = g_img[i*3+3] = + ((unsigned char)(g_camera->image[j]) << 8) + (unsigned char)(g_camera->image[j+1]); } break; Modified: code/player/trunk/utils/playerv/pv_dev_actarray.c =================================================================== --- code/player/trunk/utils/playerv/pv_dev_actarray.c 2008-08-22 18:19:44 UTC (rev 6976) +++ code/player/trunk/utils/playerv/pv_dev_actarray.c 2008-08-25 00:58:33 UTC (rev 6977) @@ -1,4 +1,4 @@ -/* +/* * PlayerViewer * Copyright (C) Andrew Howard 2002 * @@ -46,7 +46,7 @@ char section[64]; char label[64]; actarray_t *actarray; - + actarray = malloc(sizeof(actarray_t)); actarray->datatime = 0; actarray->drivername = strdup(drivername); @@ -66,7 +66,7 @@ actarray->menu = rtk_menu_create_sub(mainwnd->device_menu, label); actarray->subscribe_item = rtk_menuitem_create(actarray->menu, "Subscribe", 1); actarray->command_item = rtk_menuitem_create(actarray->menu, "Command", 1); - + // Set the initial menu state rtk_menuitem_check(actarray->subscribe_item, actarray->proxy->info.subscribed); @@ -75,7 +75,7 @@ actarray->actuator_fig_cmd = NULL; actarray->lastvalue = NULL; actarray->mainwnd = mainwnd; - + return actarray; } @@ -198,8 +198,8 @@ double min, max; double ax, ay, bx, by; double fx, fd; - int ii; + rtk_fig_t *fig; actarray_allocate(actarray, actarray->proxy->actuators_count); @@ -217,7 +217,7 @@ if (value > max) value = max; if (value < min) value = min; value = 2*(value-min)/(max-min) -1; - rtk_fig_t * fig = actarray->actuator_fig[ii]; + fig = actarray->actuator_fig[ii]; rtk_fig_show(fig, 1); rtk_fig_clear(fig); rtk_fig_origin(actarray->actuator_fig[ii], ARRAY_SPACING*ii +ARRAY_X_OFFSET, 0, 0); Modified: code/player/trunk/utils/playerv/pv_dev_gripper.c =================================================================== --- code/player/trunk/utils/playerv/pv_dev_gripper.c 2008-08-22 18:19:44 UTC (rev 6976) +++ code/player/trunk/utils/playerv/pv_dev_gripper.c 2008-08-25 00:58:33 UTC (rev 6977) @@ -105,6 +105,10 @@ // Update a gripper device void gripper_update(gripper_t *gripper) { + double gripper_length, gripper_width; + double paddle_center, paddle_length, paddle_width, paddle_pos; + double ibbx, obbx, bby; + double led_dx; int i; // Update the device subscription @@ -151,12 +155,12 @@ // draw paddles - double gripper_length = gripper->proxy->outer_size.sw; - double gripper_width = gripper->proxy->outer_size.sl; + gripper_length = gripper->proxy->outer_size.sw; + gripper_width = gripper->proxy->outer_size.sl; - double paddle_center = gripper_length * (1.0/6.0); - double paddle_length = gripper_length * (2.0/3.0); - double paddle_width = gripper_width * 0.15; + paddle_center = gripper_length * (1.0/6.0); + paddle_length = gripper_length * (2.0/3.0); + paddle_width = gripper_width * 0.15; rtk_fig_color_rgb32( gripper->grip_fig, GRIPPER_COLOR_FILL ); rtk_fig_rectangle( gripper->grip_fig, @@ -174,7 +178,7 @@ if( gripper->proxy->state == PLAYER_GRIPPER_STATE_OPEN ) { - double paddle_pos = gripper_width/2.0 - paddle_width/2.0; + paddle_pos = gripper_width/2.0 - paddle_width/2.0; rtk_fig_color_rgb32( gripper->grip_fig, GRIPPER_COLOR_FILL ); rtk_fig_rectangle(gripper->grip_fig, @@ -196,7 +200,7 @@ if( gripper->proxy->state == PLAYER_GRIPPER_STATE_CLOSED ) { - double paddle_pos = paddle_width/2.0; + paddle_pos = paddle_width/2.0; rtk_fig_color_rgb32( gripper->grip_fig, GRIPPER_COLOR_FILL ); rtk_fig_rectangle(gripper->grip_fig, @@ -227,14 +231,14 @@ // different x location for each beam - double ibbx = paddle_center - 0.3*paddle_length; - double obbx = paddle_center + 0.3*paddle_length; + ibbx = paddle_center - 0.3*paddle_length; + obbx = paddle_center + 0.3*paddle_length; // common y position - double bby = (gripper->proxy->state == PLAYER_GRIPPER_STATE_OPEN) ? gripper_width/2.0 - paddle_width: 0; + bby = (gripper->proxy->state == PLAYER_GRIPPER_STATE_OPEN) ? gripper_width/2.0 - paddle_width: 0; // size of the paddle indicator lights - double led_dx = paddle_width/2.0; + led_dx = paddle_width/2.0; if( gripper->proxy->beams & 0x00000001 ) { Modified: code/player/trunk/utils/playerv/pv_dev_vectormap.c =================================================================== --- code/player/trunk/utils/playerv/pv_dev_vectormap.c 2008-08-22 18:19:44 UTC (rev 6976) +++ code/player/trunk/utils/playerv/pv_dev_vectormap.c 2008-08-25 00:58:33 UTC (rev 6977) @@ -1,4 +1,4 @@ -/* +/* * PlayerViewer * Copyright (C) Andrew Howard 2002 * @@ -32,7 +32,7 @@ // Update the map configuration void vectormap_update_config(vectormap_t *map); -// Draw the map +// Draw the map void vectormap_draw(vectormap_t *map); // Draw a single map feature @@ -46,7 +46,7 @@ char label[64]; char section[64]; vectormap_t *map; - + map = malloc(sizeof(vectormap_t)); map->proxy = playerc_vectormap_create(client, index); map->drivername = strdup(drivername); @@ -163,12 +163,13 @@ { unsigned ii, jj; GEOSGeom feature; + uint32_t colour = 0xFF0000; + double xCenter, yCenter; rtk_fig_show(map->fig, 1); rtk_fig_clear(map->fig); // draw map data - uint32_t colour = 0xFF0000; for (ii = 0; ii < map->proxy->layers_count; ++ii) { // get a different colour for each layer the quick way, will duplicate after 6 layers @@ -186,8 +187,8 @@ // draw map extent rtk_fig_color_rgb32( map->fig, 0xFF0000 ); - double xCenter = map->proxy->extent.x1 - (map->proxy->extent.x1 - map->proxy->extent.x0)/2; - double yCenter = map->proxy->extent.y1 - (map->proxy->extent.y1 - map->proxy->extent.y0)/2; + xCenter = map->proxy->extent.x1 - (map->proxy->extent.x1 - map->proxy->extent.x0)/2; + yCenter = map->proxy->extent.y1 - (map->proxy->extent.y1 - map->proxy->extent.y0)/2; rtk_fig_rectangle( map->fig, @@ -210,7 +211,8 @@ unsigned numcoords; unsigned ii; double x,y,x2,y2; - switch (GEOSGeomTypeId(geom)) + + switch (GEOSGeomTypeId(geom)) { case GEOS_POINT: seq = GEOSGeom_getCoordSeq(geom); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <gb...@us...> - 2008-08-31 17:57:08
|
Revision: 6997 http://playerstage.svn.sourceforge.net/playerstage/?rev=6997&view=rev Author: gbiggs Date: 2008-09-01 00:57:06 +0000 (Mon, 01 Sep 2008) Log Message: ----------- Applied patches 2042945, 2004876, 1998117, 1951780 Modified Paths: -------------- code/player/trunk/config/nomad.cfg code/player/trunk/server/drivers/CMakeLists.txt code/player/trunk/server/drivers/camera/1394/CMakeLists.txt code/player/trunk/server/drivers/mixed/nomad/CMakeLists.txt code/player/trunk/server/drivers/mixed/nomad/Nclient.c code/player/trunk/server/drivers/mixed/nomad/Nclient.h code/player/trunk/server/drivers/mixed/nomad/nomad.cc code/player/trunk/server/drivers/position/CMakeLists.txt Added Paths: ----------- code/player/trunk/server/drivers/mixed/nomad/Ndirect.c code/player/trunk/server/drivers/position/motionmind/ code/player/trunk/server/drivers/position/motionmind/CMakeLists.txt code/player/trunk/server/drivers/position/motionmind/motionmind.cc code/player/trunk/server/drivers/sonar/ code/player/trunk/server/drivers/sonar/CMakeLists.txt code/player/trunk/server/drivers/sonar/aiotosonar.cc Removed Paths: ------------- code/player/trunk/server/drivers/mixed/nomad/direct.c code/player/trunk/server/drivers/mixed/nomad/mother.cc code/player/trunk/server/drivers/mixed/nomad/mother.h code/player/trunk/server/drivers/mixed/nomad/nomad.h code/player/trunk/server/drivers/mixed/nomad/nomad_position.cc code/player/trunk/server/drivers/mixed/nomad/nomad_sonar.cc Modified: code/player/trunk/config/nomad.cfg =================================================================== --- code/player/trunk/config/nomad.cfg 2008-08-30 02:45:26 UTC (rev 6996) +++ code/player/trunk/config/nomad.cfg 2008-09-01 00:57:06 UTC (rev 6997) @@ -1,34 +1,20 @@ -# Nomad config file -# Richard Vaughan, created 2004.05.13 -# $Id -# To use the generic player interfaces with a Nomad, we first need to -# instantiate and configure a single nomad device. + driver + ( + name "nomad_driver" + plugin "libnomaddriver.so" + provides ["position2d:0" "bumper:0" "sonar:0" "ir:0" "turret:::position1d:0" "compass:::position1d:1"] -# Naturally, the nomad interface uses the nomad driver by -# default. Clients can use the nomad device directly, or by using a -# standard player abstract interface - see below. - -driver -( - name "nomad" - provides ["nomad:0"] -) - -# now we can use a generic interface, using its corresponding nomad driver -driver -( - name "nomad_position" - provides ["position:0"] - requires ["nomad:0"] -) -driver -( - name "nomad_sonar" - provides ["sonar:0"] - requires ["nomad:0"] -) - -# todo - implement these interfaces -#speech:0 (driver "nomad_speech" ) -#bumper:0 (driver "nomad_bumper" ) + # Options + NOMAD_TIMEOUT_S 5 #default 5 + CYCLE_TIME_US 100000 #default 100000 + NOMAD_MAX_VEL_TRANS 200 #default 200 + NOMAD_MAX_ACC_TRANS 300 #default 300 + NOMAD_MAX_VEL_STEER 450 #default 450 + NOMAD_MAX_ACC_STEER 300 #default 300 + REAL_ROBOT 3 #default 3 - don't do anything + LOCKED 0 #default 1 - locked turret movement + ZERO 1 #default 1 - zero the robot + HOST "localhost" #default "localhost" + PORT 7019 #default 7019 + ) Modified: code/player/trunk/server/drivers/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/CMakeLists.txt 2008-08-30 02:45:26 UTC (rev 6996) +++ code/player/trunk/server/drivers/CMakeLists.txt 2008-09-01 00:57:06 UTC (rev 6997) @@ -27,6 +27,7 @@ ADD_SUBDIRECTORY (rfid) ADD_SUBDIRECTORY (service_adv) ADD_SUBDIRECTORY (shell) +ADD_SUBDIRECTORY (sonar) ADD_SUBDIRECTORY (speech) ADD_SUBDIRECTORY (vectormap) # ADD_SUBDIRECTORY (waveform) Modified: code/player/trunk/server/drivers/camera/1394/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/camera/1394/CMakeLists.txt 2008-08-30 02:45:26 UTC (rev 6996) +++ code/player/trunk/server/drivers/camera/1394/CMakeLists.txt 2008-09-01 00:57:06 UTC (rev 6997) @@ -42,4 +42,6 @@ SET (camera1394_h "${CMAKE_CURRENT_BINARY_DIR}/camera1394.h") CONFIGURE_FILE (${camera1394_h_in} ${camera1394_h}) -PLAYERDRIVER_ADD_DRIVER (camera1394 build_camera1394 LINKFLAGS "${c1394LinkFlags}" CFLAGS "${c1394CFlags}" SOURCES ${c1394Sources} ${camera1394_h}) +PLAYERDRIVER_ADD_DRIVER (camera1394 build_camera1394 LINKFLAGS "${c1394LinkFlags}" + CFLAGS "${c1394CFlags} -I${PROJECT_SOURCE_DIR}/server/drivers/blobfinder/cmvision" + SOURCES ${c1394Sources} ${camera1394_h}) Modified: code/player/trunk/server/drivers/mixed/nomad/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/mixed/nomad/CMakeLists.txt 2008-08-30 02:45:26 UTC (rev 6996) +++ code/player/trunk/server/drivers/mixed/nomad/CMakeLists.txt 2008-09-01 00:57:06 UTC (rev 6997) @@ -1,4 +1,2 @@ -PLAYERDRIVER_OPTION (nomad build_nomad OFF "Disabled by default") -SET (NOMAD_INTERFACE "Nclient.c" CACHE STRING "Choose a Nomadics interface. The default is for connection via Nserver simulator/visualisation") -MARK_AS_ADVANCED (NOMAD_INTERFACE) -PLAYERDRIVER_ADD_DRIVER (nomad build_nomad SOURCES nomad.cc nomad_position.cc nomad_sonar.cc ${NOMAD_INTERFACE}) +PLAYERDRIVER_OPTION (nomad build_nomad ON) +PLAYERDRIVER_ADD_DRIVER (nomad build_nomad SOURCES nomad.cc Nclient.c) Modified: code/player/trunk/server/drivers/mixed/nomad/Nclient.c =================================================================== --- code/player/trunk/server/drivers/mixed/nomad/Nclient.c 2008-08-30 02:45:26 UTC (rev 6996) +++ code/player/trunk/server/drivers/mixed/nomad/Nclient.c 2008-09-01 00:57:06 UTC (rev 6997) @@ -1,12 +1,3 @@ -/* This file was obtained from the nomadics respository on - Sourceforge. I understand that it was released under the GPL by the - copyright holders. Anyone with more information about the licensing - or authorship of this code, please contact Richard Vaughan - (va...@sf...). - - $Header$ -*/ - /* * Nclient.c * @@ -16,9 +7,7 @@ * */ -/* -- fixed a bug where initializing with an invalid robot id hung because - * Nclient tried to initialize sensors anyway -- rak, 16jun99 - */ +char cvsid_host_client_Nclient_c[] = "$Header: /home/cvs/host/client/Nclient.c,v 1.26 1998/05/04 17:32:54 kamason Exp $"; /* defines */ @@ -149,7 +138,6 @@ /* includes */ -#include <unistd.h> #include <stdlib.h> #include <stdio.h> #include <math.h> @@ -166,6 +154,7 @@ #include <netdb.h> #include <fcntl.h> #include <signal.h> +#include <unistd.h> #include "Nclient.h" #define DEBUG @@ -219,8 +208,8 @@ char ROBOT_MACHINE_NAME[80] = ""; int CONN_TYPE = -1; -char NOMAD_SERIAL_PORT[256] = ""; -int NOMAD_SERIAL_BAUD = -1; +char SERIAL_PORT[40] = ""; +int SERIAL_BAUD = -1; int ROBOT_TCP_PORT = -1; double LASER_CALIBRATION[8]; @@ -253,13 +242,11 @@ /* function declaration */ -//int gethostname(char *name, int len); static int posDataProcess (long *buffer, int current, PosData *posData); -static int timeDataProcess (long *buffer, int current, TimeData *theTime ); +static int timeDataProcess (long *buffer, int current, TimeData *time ); static int voltDataProcess (long *buffer, int current, unsigned char *voltCPU, unsigned char *voltMotor); static float voltConvert ( unsigned char reading , float range ); -char *convertAddr ( char *name, char *addr ); /************************* * * @@ -295,12 +282,13 @@ char addr[10]; if (!(strcmp(Host_name, ""))) - { - if (!(strcmp(SERVER_MACHINE_NAME,""))) - gethostname(Host_name, 100); - else - strcpy(Host_name, SERVER_MACHINE_NAME); - } + { + if (!(strcmp(SERVER_MACHINE_NAME,""))) + gethostname(Host_name, 100); + else + strcpy(Host_name, SERVER_MACHINE_NAME); + } + if ( ((hp = gethostbyname(Host_name)) == NULL) && ((hp = gethostbyaddr(convertAddr(Host_name,addr),4,AF_INET)) == NULL ) ) { @@ -698,7 +686,7 @@ static int process_compass_reply(struct reply_struct *this_reply) { - int i=0; + int i = 0; if (this_reply->type == ERROR_MSG) { @@ -725,7 +713,7 @@ static int process_bumper_reply(struct reply_struct *this_reply) { - int i; + int i=0; if (this_reply->type == ERROR_MSG) { @@ -993,7 +981,7 @@ if (first) { - fprintf(stderr, "Nclient version 2.7\n"); + fprintf(stderr, "Nclient version 2.6.11\n"); fprintf(stderr, "Copyright 1991-1998, Nomadic Technologies, Inc.\n"); first = 0; } @@ -1013,11 +1001,6 @@ { error = process_socket_reply(&the_reply); - /* if there was an error, we must not initialize the sensors, - * but return an error immediately instead */ - if (error == 0) - return 0; - /* initialize clients Smask to match the one on the server side */ init_sensors(); @@ -1114,7 +1097,7 @@ the_request.mesg[0] = t_sp; the_request.mesg[1] = s_sp; the_request.mesg[2] = r_sp; - + if (ipc_comm(&the_request, &the_reply)) { return(process_state_reply(&the_reply)); @@ -1424,7 +1407,7 @@ int tk(char *talk_string) { the_request.type = TK_MSG; - the_request.size = (strlen(talk_string)+4)/4; + the_request.size = (strlen(talk_string)+3)/4; strcpy((char *)the_request.mesg, talk_string); if (ipc_comm(&the_request, &the_reply)) @@ -3587,9 +3570,9 @@ * CALLS: * CALLED BY: ***************/ -static int timeDataProcess ( long *buffer, int current, TimeData *theTime ) +static int timeDataProcess ( long *buffer, int current, TimeData *time ) { - *theTime = (unsigned long) buffer[current]; + *time = (unsigned long) buffer[current]; return ( current + 1 ); } Modified: code/player/trunk/server/drivers/mixed/nomad/Nclient.h =================================================================== --- code/player/trunk/server/drivers/mixed/nomad/Nclient.h 2008-08-30 02:45:26 UTC (rev 6996) +++ code/player/trunk/server/drivers/mixed/nomad/Nclient.h 2008-09-01 00:57:06 UTC (rev 6997) @@ -8,7 +8,7 @@ * */ -/* $Header$ */ +/* $Header: /home/cvs/host/client/Nclient.h,v 1.25 1998/05/04 17:34:52 kamason Exp $ */ #ifndef _HOST_CLIENT_NCLIENT_H_ #define _HOST_CLIENT_NCLIENT_H_ @@ -92,9 +92,7 @@ #define STATE_CONF_STEER 36 #define STATE_CONF_TURRET 37 #define STATE_VEL_TRANS 38 -#define STATE_VEL_RIGHT 38 /* for scout */ #define STATE_VEL_STEER 39 -#define STATE_VEL_LEFT 39 /* for scout */ #define STATE_VEL_TURRET 40 #define STATE_MOTOR_STATUS 41 #define STATE_LASER 42 @@ -188,12 +186,6 @@ #define ARM_WS 41 #define ARM_MV 42 -/* - * function prototypes for arm - */ -long arm_mv(long l_mode, long l_v, long g_mode, long g_v); -long arm_ws(short lift, short grip, long timeout, long *time_remain); -long arm_zr(short mode); /* * For requesting the PosData the following defines should be used. @@ -271,14 +263,6 @@ #define USER_BUFFER_LENGTH 0xFFFF -/* these definitions apply to the Scout and SuperScout */ -#define ROTATION_CONSTANT 0.118597 /* inches/degree (known to 100 ppm) */ - -#define RIGHT(trans, steer) (trans + (int)((float)steer*ROTATION_CONSTANT)) -#define LEFT(trans, steer) (trans - (int)((float)steer*ROTATION_CONSTANT)) - -#define scout_vm(trans, steer) vm(RIGHT(trans, steer), LEFT(trans, steer), 0) - /******************** * * * Type definitions * @@ -483,8 +467,8 @@ * ROBOT_MACHINE_NAME * usually don't need to be changed: * CONN_TYPE - * NOMAD_SERIAL_PORT - * NOMAD_SERIAL_BAUD + * SERIAL_PORT + * SERIAL_BAUD * ROBOT_TCP_PORT * * If an application program should run with Nclient.o and Ndirect.o @@ -520,12 +504,12 @@ /* SERIAL_PORT is a string containing the filename of the serial port you * choose to communicate to the robot on. The default is "/dev/ttyS0". */ -//extern char NOMAD_SERIAL_PORT[256]; +extern char SERIAL_PORT[40]; /* SERIAL_BAUD is the baud rate to set the serial communication to. It * defaults to 9600. */ -//extern int NOMAD_SERIAL_BAUD; +extern int SERIAL_BAUD; /* ROBOT_TCP_PORT is the port number the robot listens on for request of * connection. It defaults (and should always be set) to 65001 for the @@ -583,6 +567,8 @@ */ int connect_robot(long robot_id, ...); +int old_connect_robot(long robot_id); /* added by Deryck Morales */ + /* * disconnect_robot - requests the server to close the connect with robot * with id = robot_id. Added: code/player/trunk/server/drivers/mixed/nomad/Ndirect.c =================================================================== --- code/player/trunk/server/drivers/mixed/nomad/Ndirect.c (rev 0) +++ code/player/trunk/server/drivers/mixed/nomad/Ndirect.c 2008-09-01 00:57:06 UTC (rev 6997) @@ -0,0 +1,4161 @@ +/* + * Ndirect.c + * + * Implementation file for direct connection to robot, bypassing the need for + * the Nserver program. + * + * Copyright 1991-1998, Nomadic Technologies, Inc. + * + */ + +char cvsid_host_client_Ndirect_c[] = "$Header: /home/cvs/host/client/Ndirect.c,v 1.16 1998/05/04 17:34:53 kamason Exp $"; + +/* includes */ + +#include <arpa/inet.h> +#include <fcntl.h> +#include <termios.h> +#include <signal.h> +#include <memory.h> +#include <errno.h> +#include <netdb.h> +#include <netinet/in.h> +#include <stdarg.h> +#include <stdio.h> +#include <unistd.h> +#include <sys/types.h> +#include <sys/socket.h> +#include <sys/time.h> + +#include "Nclient.h" + +/* defines */ + +/* from command type.h */ + +#define AC 1 +#define SP 2 +#define PR 3 +#define PA 4 +#define VM 5 +#define MV 43 +#define CT 6 +#define GS 7 +#define NAK 8 +#define ST 9 +#define LP 10 +#define TK 11 +#define DP 12 +#define ZR 13 +#define CONF_IR 14 +#define CONF_SN 15 +#define CONF_CP 16 +#define CONF_LS 17 +#define CONF_TM 18 +#define GET_IR 19 +#define GET_SN 20 +#define GET_RC 21 +#define GET_RV 22 +#define GET_RA 23 +#define GET_CP 24 +#define GET_LS 25 +#define SETUP_LS 26 +#define GET_BP 27 + +#define CONF_SG 28 +#define GET_SG 29 + +#define DA 30 +#define WS 31 + +#define ADD_OBS 32 +#define DELETE_OBS 33 +#define MOVE_OBS 34 + +#define CONF_SER 35 +#define PLACE_ROBOT 36 + +#define NUM_COMMANDS 36 + +#define SPECIAL 128 + +/* error types */ +#define SERIAL_OPEN_ERROR 1 +#define SERIAL_WRITE_ERROR 2 +#define SERIAL_READ_ERROR 3 +#define SERIAL_PKG_ERROR 4 +#define SERIAL_TIMEOUT_ERROR 5 + +/* serial setting */ +#define RE_XMIT 0 /* not re-transmit when failed */ +#define NORMAL_TIMEOUT 1 /* 1 second */ +#define CONNECT_TIMEOUT 10 /* 10 seconds */ +#define SPECIAL_TIMEOUT 30 /* used for user define package */ + +/* + * Define the length of the user buffer (Maximal short). + * Due to Protocol bytes, the effective length is 65526 + */ + +#define USER_BUFFER_LENGTH 0xFFFF + +/* from pos.h */ + +/* + * these macros enable the user to determine if the pos-attachment + * is desired for a specific sensor, the argument is Smask[ SMASK_POS_DATA ] + * to avoid overlap with Nclient.h suffix I for internal + */ + +#define POS_INFRARED_PI(x) ( ( (x) & POS_INFRARED ) ? 1 : 0 ) +#define POS_SONAR_PI(x) ( ( (x) & POS_SONAR ) ? 1 : 0 ) +#define POS_BUMPER_PI(x) ( ( (x) & POS_BUMPER ) ? 1 : 0 ) +#define POS_LASER_PI(x) ( ( (x) & POS_LASER ) ? 1 : 0 ) +#define POS_COMPASS_PI(x) ( ( (x) & POS_COMPASS ) ? 1 : 0 ) + + +/* from datatype.h */ + +/* to build a long out ouf four bytes */ + +#define LONG_B(x,y,z,q) ((((x) << 24) & 0xFF000000) | \ + (((y) << 16) & 0x00FF0000) | \ + (((z) << 8) & 0x0000FF00) | \ + ( (q) & 0x000000FF) ) + +/* + * The voltages have different ranges to account for the fact that the + * CPU measurement is taken after lossage on the slip-ring. + */ + +#define RANGE_CPU_VOLTAGE 12.0 +#define RANGE_MOTOR_VOLTAGE 12.85 + +/******************** + * * + * Type definitions * + * * + ********************/ + +/* + * PosDataAll is a struct that contains the Pos information for + * all sensors. It is used to pass/store the Pos info within the + * server. It contains the laser, although the laser is not + * used in the dual ported ram. + */ + +typedef struct _PosDataAll +{ + PosData infrared [INFRAREDS]; + PosData sonar [SONARS ]; + PosData bumper; + PosData laser; + PosData compass; + +} PosDataAll; + + +/******************** + * * + * Global Variables * + * * + ********************/ + +/* Added by Deryck Morales 7.2002 */ +extern int ROBOT_MODEL; +int CONN_TYPE = 2; +char SERIAL_PORT[40] = "/dev/ttyS0"; +int SERIAL_BAUD = 9600; +char ROBOT_MACHINE_NAME[80] = ""; +int ROBOT_TCP_PORT = 65001; + +long State[NUM_STATE]; /* State reading */ +int Smask[NUM_MASK]; /* Sensor mask */ +int Laser[2*NUM_LASER+1]; /* Laser reading */ + + +/* connect_type == 1 ---> serial */ +/* connect_type == 2 ---> socket */ +int connect_type = 1; +int model; +char *device; +int conn_value; +int DEFAULT_SERIAL_BAUD = 38400; +int DEFAULT_ROBOT_TCP_PORT = 65001; +double LASER_CALIBRATION[8] = { -0.003470, 0.000008, 0.011963, 0.001830, + 27.5535913, 0.000428, 0.031102, -0.444624 }; +double LASER_OFFSET[2] = { 0, 0 }; + +/* dummy variables to stay compatible with Nclient.c */ + +char SERVER_MACHINE_NAME[80] = ""; +int SERV_TCP_PORT = -1; +char Host_name[255] = ""; + +/******************* + * * + * Local Variables * + * * + *******************/ + +static int Fd=-1; +static unsigned char buf[BUFSIZE]; +static unsigned char *bufp, *bufe; +static int errorp = 0; +static int wait_time; +static int usedSmask[NUM_MASK]; /* mask vector */ +static int Robot_id = -1; + +/* although called special, it is the user buffer */ +static unsigned char *special_buffer; +static unsigned short special_answer_size; + +/* this is where all the incoming posData is stored */ +static PosDataAll posDataAll; +static unsigned long posDataTime; + +/* for the voltages of motor/CPU as raw data */ +static unsigned char voltageCPU; +static unsigned char voltageMotor; + +/* the laser mode */ +static int laser_mode = 51; + +/******************* + * * + * Functions (fwd) * + * * + *******************/ + +/* function declarations */ + +static long posLongExtract ( unsigned char *inbuf ); +static unsigned long posUnsignedLongExtract( unsigned char *inbuf ); +static int posPackageProcess ( unsigned char *inbuf, PosData *posData ); +static int timePackageProcess ( unsigned char *inbuf, unsigned long *timeS ); +static int voltPackageProcess ( unsigned char *inbuf, + unsigned char *voltCPU, + unsigned char *voltMotor); +static float voltConvert ( unsigned char reading , float range ); + + + +/******************************************************* + * * + * Helper functions for manipulating bytes and numbers * + * * + *******************************************************/ + +static int low_half(unsigned char num) +{ + return (num % 16); +} + +static int high_half(unsigned char num) +{ + return (num / 16); +} + +/* +static unsigned char high_byte_signed(int n) +{ + int sign_num; + + if (n < 0) sign_num = 128; else sign_num = 0; + return (sign_num + (abs(n) / 256)); +} +*/ +/* +static unsigned char low_byte_signed(int n) +{ + return (abs(n) % 256); +} +*/ +/* +static unsigned char high_byte_unsigned(int n) +{ + return (n / 256); +} +*/ +/* +static unsigned char low_byte_unsigned(int n) +{ + return (n % 256); +} +*/ +static void signed_int_to_two_bytes(int n, unsigned char *byte_ptr) +{ + int sign_num; + + *byte_ptr = (unsigned char)(abs(n) % 256); + byte_ptr++; + if (n < 0) sign_num = 128; else sign_num = 0; + *byte_ptr = (unsigned char)(sign_num + (abs(n) / 256)); +} + +static void unsigned_int_to_two_bytes(int n, unsigned char *byte_ptr) +{ + *byte_ptr = (unsigned char)(abs(n) % 256); + byte_ptr++; + *byte_ptr = (unsigned char)(abs(n) / 256); +} + +static int two_bytes_to_signed_int(unsigned char low_byte, + unsigned char high_byte) +{ + int num; + + if (high_byte > 127) + num = (-256 * (high_byte - 128)) - low_byte; + else + num = 256 * high_byte + low_byte; + return (num); +} + +static unsigned int two_bytes_to_unsigned_int(unsigned char low_byte, + unsigned char high_byte) +{ + unsigned int num; + + num = 256 * high_byte + low_byte; + return (num); +} + +static long combine_bumper_vector(unsigned char b1, + unsigned char b2, + unsigned char b3) +{ + long num; + + num = b1 + (b2 * 256) + (b3 * 65536); + return (num); +} + +static unsigned char bits_to_byte(char bt0, char bt1, char bt2, char bt3, + char bt4, char bt5, char bt6, char bt7) +{ + unsigned char rbyte; + + rbyte = (unsigned char)(bt0 + (2*bt1) + (4*bt2) + (8*bt3) + (16*bt4) + + (32*bt5) + (64*bt6) + (128*bt7)); + return (rbyte); +} + +static int serial_ready (int fd, int wait) +{ + fd_set lfdvar; + int ready; + struct timeval timeout; + + FD_ZERO(&lfdvar); + FD_SET(fd, &lfdvar); + + timeout.tv_sec = wait; + timeout.tv_usec = 0; + + ready = select(fd+1, &lfdvar, NULL, NULL, &timeout); + return(ready); +} + +/* + * creates a package by adding initial byte, checksum, and end byte + * and send the package to robot + */ +static int Write_Pkg(int fd, unsigned char *outbuf) +{ + int i, outbufLen, chk_sum; + int nleft, nwritten; + + /* create a package */ + outbuf[0] = 1; + chk_sum = 0; + outbufLen = outbuf[1]+1; + for (i=0; i<=outbufLen; i++) + chk_sum = chk_sum + outbuf[i]; + chk_sum = chk_sum % 256; + outbufLen++; + outbuf[outbufLen] = chk_sum; + outbufLen++; + outbuf[outbufLen] = 92; + outbufLen++; + + /* send package */ + for (nleft = outbufLen; nleft > 0; ) { + nwritten = write(fd, outbuf, nleft); + if (nwritten <= 0) { + errorp = SERIAL_WRITE_ERROR; + return(FALSE); + } + nleft = nleft - nwritten; + outbuf = outbuf + nwritten; + } + return(TRUE); +} + + +/* + * read response from the robot + */ +static unsigned char buf_fill(int fd, int conn_type) +{ + int n; + + if (conn_type == 1) + { + n = read(fd, buf, BUFSIZE); + if (n < 0) { + printf("error reading serial port\n"); + errorp = SERIAL_READ_ERROR; + return(0); + } + else { + if (n == 0) { + printf("serial port read timeout error\n"); + errorp = SERIAL_TIMEOUT_ERROR; + return(0); + } + } + } else { + if (serial_ready(Fd, 100)) { + n = read(Fd, buf, BUFSIZE); + if (n < 0) { + printf("TCP/IP communication broken.\n"); + errorp = SERIAL_READ_ERROR; + return 0; + } else { + if (n == 0) { + printf("TCP/IP select/read error\n"); + errorp = SERIAL_READ_ERROR; + return(0); + } + } + } else { + printf("TCP/IP read timeout error.\n"); + errorp = SERIAL_TIMEOUT_ERROR; + return 0; + } + } + + bufp = &buf[1]; + bufe = buf + n; + return(buf[0]); +} + +static unsigned char GETC(int fd, int conn_type) +{ + if (bufp<bufe) + return(*bufp++); + return(buf_fill(fd, conn_type)); +} + +/* + * getting package from robot and + * check for package error: initial byte = 1, end byte = 92, check sum + */ +static int Read_Pkg(int fd, int conn_type, unsigned char *inbuf) +{ + int i, length=0, chk_sum=0; + unsigned char ichar, ichar2; + + if (!(serial_ready (fd, wait_time))) { + errorp = SERIAL_TIMEOUT_ERROR; + return(FALSE); + } + + errorp = 0; + + /* read the begin packet character, it should be 1 */ + ichar = (unsigned char)GETC(fd, conn_type); + if (!errorp) { + if (ichar != 1) { + printf("start byte error: %u\n", ichar); + errorp = SERIAL_PKG_ERROR; + } + else { + chk_sum = 1; + } + } + + if (!errorp) { + /* read length, it should be >0 */ + ichar = GETC(fd, conn_type); + if (!errorp) { + chk_sum = chk_sum + (int)ichar; + } + ichar2 = GETC(fd, conn_type); + if (!errorp) { + length = two_bytes_to_unsigned_int (ichar, ichar2); + if (length < 1) { + printf("length byte error\n"); + errorp = SERIAL_PKG_ERROR; + } + else { + chk_sum = chk_sum + (int)ichar2; + } + } + } + + /* get the data portion of the message */ + i = 0; + while ((!errorp) && (i<=length)) { + ichar = GETC(fd, conn_type); + if (!errorp) { + inbuf[i] = ichar; + /* printf("%u\n", ichar); */ + chk_sum = chk_sum + (int)(ichar); + } + i++; + } + + if (!errorp) { + /* check chk_sum and end_pkg */ + if (((chk_sum - inbuf[length-1] - 92) % 256) != inbuf[length-1]) { + printf("check sum error\n"); + errorp = SERIAL_PKG_ERROR; + } + + if (inbuf[length] != 92) { + printf("packet end error\n"); + errorp = SERIAL_PKG_ERROR; + } + } + + if ((errorp) && (errorp != SERIAL_TIMEOUT_ERROR)) { + printf("emptying buffer\n"); + buf_fill(Fd, conn_type); /* read everything else in the serial line into + buffer */ + bufp = bufe; /* and flush it */ + } + + if (errorp) + return(FALSE); + else + return (TRUE); +} + +/********************************************************* + * + * Laser Calibration Stuff + * + *********************************************************/ + +/* Transformation function accordingly to the calibration */ +/* xi1 = pixel; yi1 = scanline */ +static void ProjectPhy(double xi1, double yi1, double *x, double *y) +{ + double xi,yi; + double den; + + xi = xi1 - 254.5; + yi = yi1 - 240.5; + + den = (LASER_CALIBRATION[0]*xi + LASER_CALIBRATION[1]*yi + 1); + + *x = (LASER_CALIBRATION[2]*xi + LASER_CALIBRATION[3]*yi + + LASER_CALIBRATION[4])/den + LASER_OFFSET[0]; + *y = (LASER_CALIBRATION[5]*xi + LASER_CALIBRATION[6]*yi + + LASER_CALIBRATION[7])/den + LASER_OFFSET[1]; +} + +static void convert_laser(int *laser) +{ + int i, num_points, offset, interval; + double line_num; + double laserx[483], lasery[483]; + + num_points = laser[0]; + interval = NUM_LASER/(num_points-1); + offset = 3 + (NUM_LASER-(num_points * interval))/2; + for (i=1; i<=num_points; i++) { + line_num = (double)(offset+(i-1)*interval); + ProjectPhy((double)laser[i], line_num, &laserx[i], &lasery[i]); + } + for (i=1; i<=num_points; i++) { + laser[2*i-1] = (int)(laserx[i]*10.0); + laser[2*i] = (int)(lasery[i]*10.0); + } + return; +} + +/********************************************************* + * + * Processing different types of packages received from the robot + * + *********************************************************/ + +/* process the response received from the robot which + encodes the state of the robot according to the mask */ + +static void Process_State_Pkg(unsigned char inbuf[BUFSIZE]) +{ + int i, byte_count = 1; + int low_half_used = FALSE; + + /* infrared */ + for (i = STATE_IR_0 ; i <= STATE_IR_15; i++) + if (usedSmask[i] > 0) + { + if (low_half_used == FALSE) + { + State[i] = low_half(inbuf[byte_count]); + low_half_used = TRUE; + } + else + { + State[i] = high_half(inbuf[byte_count]); + byte_count++; + low_half_used = FALSE; + } + } + if (low_half_used == TRUE) + byte_count++; + + /* + * if the pos attachment was required we read it + */ + + if (POS_INFRARED_PI(usedSmask[SMASK_POS_DATA])) + for (i = 0; i < INFRAREDS; i++) + if (usedSmask[SMASK_IR_1 + i] > 0) + byte_count += posPackageProcess(&inbuf[byte_count], + &(posDataAll.infrared[i])); + + /* sonars */ + for (i = STATE_SONAR_0; i <= STATE_SONAR_15; i++) { + if ( usedSmask[i] > 0 ) + { + State[i] = inbuf[byte_count]; + byte_count++; + } + } + + /* + * if the pos attachment was required we read it + */ + + if (POS_SONAR_PI(usedSmask[SMASK_POS_DATA])) + for (i = 0; i < SONARS; i++) + if (usedSmask[SMASK_SONAR_1 + i] > 0) + byte_count += posPackageProcess(&inbuf[byte_count], + &(posDataAll.sonar[i])); + + if (usedSmask[ SMASK_BUMPER ] > 0) + { + if (model == MODEL_SCOUT) + { + State[ STATE_BUMPER ] = combine_bumper_vector(inbuf[byte_count + 0], + inbuf[byte_count + 1], + inbuf[byte_count + 2]); + } + else + { + State[ STATE_BUMPER ] = combine_bumper_vector(inbuf[byte_count + 2], + inbuf[byte_count + 1], + inbuf[byte_count + 0]); + } + + byte_count = byte_count + 3; + + /* + * if the position attachment was requested for the bumper + * we have to unpack the package. + */ + + if (POS_BUMPER_PI(usedSmask[SMASK_POS_DATA])) + byte_count += posPackageProcess(&inbuf[byte_count], + &(posDataAll.bumper)); + } + + /* the position data */ + + if (usedSmask[SMASK_CONF_X] > 0) + { + State[STATE_CONF_X] = two_bytes_to_signed_int(inbuf[byte_count], + inbuf[byte_count+1]); + byte_count = byte_count + 2; + } + + if (usedSmask[SMASK_CONF_Y] > 0) + { + State[STATE_CONF_Y] = two_bytes_to_signed_int(inbuf[byte_count], + inbuf[byte_count+1]); + byte_count = byte_count + 2; + } + + if (usedSmask[SMASK_CONF_STEER] > 0) + { + State[STATE_CONF_STEER] = two_bytes_to_signed_int(inbuf[byte_count], + inbuf[byte_count+1]); + byte_count = byte_count + 2; + } + + if (usedSmask[SMASK_CONF_TURRET] > 0) + { + State[STATE_CONF_TURRET] = two_bytes_to_signed_int(inbuf[byte_count], + inbuf[byte_count+1]); + byte_count = byte_count + 2; + } + + /* the velocities */ + + if (usedSmask[SMASK_VEL_TRANS] > 0) + { + State[STATE_VEL_TRANS] = two_bytes_to_signed_int(inbuf[byte_count], + inbuf[byte_count+1]); + byte_count = byte_count + 2; + } + + if (usedSmask[SMASK_VEL_STEER] > 0) + { + State[SMASK_VEL_STEER] = two_bytes_to_signed_int(inbuf[byte_count], + inbuf[byte_count+1]); + byte_count = byte_count + 2; + } + + if (usedSmask[SMASK_VEL_TURRET] > 0) + { + State[STATE_VEL_TURRET] = two_bytes_to_signed_int(inbuf[byte_count], + inbuf[byte_count+1]); + byte_count = byte_count + 2; + } + + /* the compass value */ + + if (usedSmask[SMASK_COMPASS] > 0) + { + State[STATE_COMPASS] = two_bytes_to_signed_int(inbuf[byte_count], + inbuf[byte_count+1]); + byte_count = byte_count + 2; + + /* + * if the position attachment was requested for the compass + * we have to unpack the package. + */ + + if (POS_COMPASS_PI(usedSmask[SMASK_POS_DATA])) + byte_count += posPackageProcess(&inbuf[byte_count], + &(posDataAll.compass)); + } + + /* laser */ + if (usedSmask[SMASK_LASER] > 0) + { + /* the number of points */ + Laser[0] = two_bytes_to_unsigned_int(inbuf[byte_count], + inbuf[byte_count+1]); + byte_count = byte_count + 2; + + /* check the laser mode */ + if ((laser_mode&0x1e) == 0) /* Line mode */ + { + if (Laser[0] > NUM_LASER/2) + { + printf("error in processing laser reply (1).\n"); + errorp = SERIAL_READ_ERROR; + Laser[0] = 0; + return; + } + for (i=1; i<=4*Laser[0]; i++) + { + Laser[i] = two_bytes_to_signed_int(inbuf[byte_count], + inbuf[byte_count+1]); + byte_count = byte_count+2; + } + } + else /* Points of some kind */ + { + if (Laser[0] > NUM_LASER) + { + printf("error in processing laser reply (2).\n"); + errorp = SERIAL_READ_ERROR; + Laser[0] = 0; + return; + } + for (i=1; i<=Laser[0]; i++) + { + Laser[i] = two_bytes_to_unsigned_int(inbuf[byte_count], + inbuf[byte_count+1]); + byte_count = byte_count+2; + } + } + if ((laser_mode&0x1e) == 19) + convert_laser(Laser); + + /* + * if the position attachment was requested for the laser + * we have to get it from somewhere else + */ + + if (POS_LASER_PI(usedSmask[SMASK_POS_DATA])) + byte_count += posPackageProcess(&inbuf[byte_count], + &(posDataAll.laser)); + } + /* motor active */ + State[STATE_MOTOR_STATUS] = (long)inbuf[byte_count++]; + + /* process the 6811 time */ + byte_count += timePackageProcess(&inbuf[byte_count], &posDataTime); + + /* process the voltages of motor/CPU */ + byte_count += voltPackageProcess(&inbuf[byte_count], + &voltageCPU, &voltageMotor); +} + +/* process the response from the robot which encodes the + active infrared reading */ + +static void Process_Infrared_Pkg(unsigned char inbuf[BUFSIZE]) +{ + int i, byte_count = 1; + int low_half_used = FALSE; + + /* + * the ir datum from one sensor is only a nibble, + * two of them are merged into one byte + */ + + for (i = STATE_IR_0 ; i <= STATE_IR_15; i++) + if (low_half_used == FALSE) + { + State[i] = low_half(inbuf[byte_count]); + low_half_used = TRUE; + } + else + { + State[i] = high_half(inbuf[byte_count]); + byte_count++; + low_half_used = FALSE; + } + + /* align with next byte */ + if ( low_half_used ) + byte_count++; + + /* + * if the pos attachment was required we read it + */ + + if ( POS_INFRARED_PI ( usedSmask[ SMASK_POS_DATA ] ) ) + { + for (i=0; i<16; i++) + byte_count += posPackageProcess ( &inbuf[byte_count], + &( posDataAll.infrared[i] ) ); + } + + /* extract the time data for the 6811 */ + byte_count += timePackageProcess ( &inbuf[byte_count], &posDataTime ); +} + +/* process the response from the robot which encodes the + active sonar reading */ + +static void Process_Sonar_Pkg(unsigned char inbuf[BUFSIZE]) +{ + int i, byte_count = 1; + + /* + * read the sensory data from the buffer + */ + + for (i = STATE_SONAR_0; i <= STATE_SONAR_15; i++) + { + State[i] = inbuf[byte_count]; + byte_count++; + } + + /* + * if the pos attachment was required we read it + */ + + if ( POS_SONAR_PI ( usedSmask[ SMASK_POS_DATA ]) ) + for (i=0; i<16; i++) + byte_count += posPackageProcess ( &inbuf[byte_count], + &( posDataAll.sonar[i] ) ); + + /* extract the time data for the 6811 */ + byte_count += timePackageProcess ( &inbuf[byte_count], &posDataTime ); +} + +/* process the response from the robot which encodes the + configuration of the robot */ + +static void Process_Configuration_Pkg(unsigned char inbuf[BUFSIZE]) +{ + int byte_count = 1; + + State[ STATE_CONF_X ] = two_bytes_to_signed_int(inbuf[byte_count], + inbuf[byte_count+1]); + byte_count = byte_count + 2; + + State[ STATE_CONF_Y ] = two_bytes_to_signed_int(inbuf[byte_count], + inbuf[byte_count+1]); + byte_count = byte_count + 2; + + State[ STATE_CONF_STEER ] = two_bytes_to_signed_int(inbuf[byte_count], + inbuf[byte_count+1]); + byte_count = byte_count + 2; + + State[ STATE_CONF_TURRET ] = two_bytes_to_signed_int(inbuf[byte_count], + inbuf[byte_count+1]); +} + +static void Process_Velocity_Pkg(unsigned char inbuf[BUFSIZE]) +{ + int byte_count = 1; + + State[ STATE_VEL_TRANS ] = two_bytes_to_signed_int(inbuf[byte_count], + inbuf[byte_count+1]); + byte_count = byte_count + 2; + + State[ STATE_VEL_STEER ] = two_bytes_to_signed_int(inbuf[byte_count], + inbuf[byte_count+1]); + byte_count = byte_count + 2; + + State[ STATE_VEL_TURRET ] = two_bytes_to_signed_int(inbuf[byte_count], + inbuf[byte_count+1]); +} + +static void Process_Acceleration_Pkg(unsigned char inbuf[BUFSIZE]) +{ +} + +/* process the response from the robot which encodes the + compass reading of the robot */ + +static void Process_Compass_Pkg(unsigned char inbuf[BUFSIZE]) +{ + int byte_count = 1; + + State[ STATE_COMPASS ] = two_bytes_to_unsigned_int(inbuf[byte_count], + inbuf[byte_count+1]); + byte_count +=2; + + /* + * if the position attachment was requested for the compass + * we have to unpack the package. + */ + + if ( POS_COMPASS_PI ( usedSmask[ SMASK_POS_DATA ] ) ) + byte_count += posPackageProcess ( &inbuf[byte_count], + &( posDataAll.compass ) ); + + /* extract the time data for the 6811 */ + byte_count += timePackageProcess ( &inbuf[byte_count], &posDataTime ); +} + +/* process the response from the robot which encodes the + compass reading of the robot */ + +static void Process_Compass_Conf_Pkg(unsigned char inbuf[BUFSIZE]) +{ + int byte_count = 1; + + printf("compass calibration score x: %d y: %d z: %d\n", + inbuf[byte_count], + inbuf[byte_count+1], + inbuf[byte_count+2]); +} + +/* process the response from the robot which encodes the + bumper reading of the robot */ + +static void Process_Bumper_Pkg(unsigned char inbuf[BUFSIZE]) +{ + int byte_count = 1; + + if (model == MODEL_SCOUT) + { + State[ STATE_BUMPER ] = combine_bumper_vector(inbuf[byte_count + 0], + inbuf[byte_count + 1], + inbuf[byte_count + 2]); + } + else + { + State[ STATE_BUMPER ] = combine_bumper_vector(inbuf[byte_count + 2], + inbuf[byte_count + 1], + inbuf[byte_count + 0]); + } + + byte_count +=3; + + /* + * if the position attachment was requested for the bumper + * we have to unpack the package. + */ + + if ( POS_BUMPER_PI ( usedSmask[ SMASK_POS_DATA ] ) ) + byte_count += posPackageProcess ( &inbuf[byte_count], + &( posDataAll.bumper ) ); + + /* extract the time data for the 6811 */ + byte_count += timePackageProcess ( &inbuf[byte_count], &posDataTime ); +} + +static void Process_Laser_Point_Pkg(unsigned char inbuf[BUFSIZE]) +{ + int i, byte_count = 1; + + Laser[0]=two_bytes_to_unsigned_int(inbuf[byte_count],inbuf[byte_count+1]); + byte_count = byte_count+2; + for (i=1; i<=Laser[0]; i++) { + Laser[i] = two_bytes_to_signed_int(inbuf[byte_count], inbuf[byte_count+1]); + byte_count = byte_count+2; + } + convert_laser(Laser); + + Laser[0] = inbuf[byte_count] + 256 * inbuf[byte_count+1]; + byte_count = byte_count + 2; + + if ( Laser[0] > NUM_LASER ) { + printf("error in processing laser point reply\n"); + errorp = SERIAL_READ_ERROR; + Laser[0] = 0; + return; + } + for (i=1; i<=Laser[0]; i++) { + Laser[i] = two_bytes_to_unsigned_int(inbuf[byte_count], + inbuf[byte_count+1]); + byte_count = byte_count+2; + } + if ((laser_mode == 51) || (laser_mode == 50) || (laser_mode == 19)) + convert_laser(Laser); + + /* + * if the position attachment was requested for the laser + * we have to unpack the package. + */ + + if ( POS_LASER_PI ( usedSmask[ SMASK_POS_DATA ] ) ) + byte_count += posPackageProcess ( &inbuf[byte_count], + &( posDataAll.laser ) ); + + /* extract the time data for the 6811 */ + byte_count += timePackageProcess ( &inbuf[byte_count], &posDataTime ); +} + +static void Process_Laser_Line_Pkg(unsigned char inbuf[BUFSIZE]) +{ + int i, byte_count = 1; + + Laser[0] = inbuf[byte_count] + 256 * inbuf[byte_count+1]; + byte_count = byte_count + 2; + + if (Laser[0] > NUM_LASER) { + printf("error in processing laser line reply\n"); + errorp = SERIAL_READ_ERROR; + Laser[0] = 0; + return; + } + for (i=1; i<=4*Laser[0]; i++) { + Laser[i] = two_bytes_to_signed_int(inbuf[byte_count], + inbuf[byte_count+1]); + byte_count = byte_count+2; + } + + /* + * if the position attachment was requested for the laser + * we have to unpack the package. + */ + + if ( POS_LASER_PI ( usedSmask[ SMASK_POS_DATA ] ) ) + byte_count += posPackageProcess ( &inbuf[byte_count], + &( posDataAll.laser ) ); + + /* extract the time data for the 6811 */ + byte_count += timePackageProcess ( &inbuf[byte_count], &posDataTime ); +} + +/* process the response from the robot which encodes special information */ + +static void Process_Special_Pkg(unsigned char inbuf[BUFSIZE]) +{ + int data_size, i, byte_count = 1; + + data_size = two_bytes_to_unsigned_int(inbuf[1],inbuf[2]); + special_answer_size = ( unsigned short ) data_size; + + if (data_size > MAX_USER_BUF) + data_size = MAX_USER_BUF; + + if ( special_buffer != (unsigned char *)NULL) + { + for (i=0; i<data_size; i++) + { + special_buffer[i] = inbuf[i+1]; + byte_count++; + } + } + else + printf("Data buffer for user package is NULL pointer\n"); +} + +static int Process_Robot_Resp(unsigned char inbuf[BUFSIZE]) +{ + switch (inbuf[0]) { /* type of the returned package */ + case AC: + case SP: + case PR: + case PA: + case VM: + case MV: + case CT: + case GS: + case ST: + case LP: + case DP: + case DA: + case WS: + case ZR: + case TK: + case CONF_IR: + case CONF_SN: + case CONF_LS: + case CONF_TM: + case CONF_SG: + case CONF_SER: + case SETUP_LS: + Process_State_Pkg(inbuf); + break; + case CONF_CP: + Process_Compass_Conf_Pkg(inbuf); + break; + case NAK: /* Nak */ + printf("Nak\n"); + break; + case GET_IR: /* Infrared */ + Process_Infrared_Pkg(inbuf); + break; + case GET_SN: /* Sonar */ + Process_Sonar_Pkg(inbuf); + break; + case GET_RC: /* Configuration */ + Process_Configuration_Pkg(inbuf); + break; + case GET_RV: /* Velocity */ + Process_Velocity_Pkg(inbuf); + break; + case GET_RA: /* Acceleration */ + Process_Acceleration_Pkg(inbuf); + break; + case GET_CP: /* Compass */ + Process_Compass_Pkg(inbuf); + break; + case GET_LS: /* Laser */ + Process_Laser_Point_Pkg(inbuf); + break; + case GET_BP: /* Bumper */ + Process_Bumper_Pkg(inbuf); + break; + case GET_SG: /* Laser line mode */ + Process_Laser_Line_Pkg(inbuf); + break; + case SPECIAL: /* User */ + Process_Special_Pkg(inbuf); + break; + default: + printf("Invalid Robot Response\n"); + return(FALSE); + break; + } + return(TRUE); +} + +static int Comm_Robot(int fd, unsigned char command[BUFSIZE]) +{ + unsigned char response[BUFSIZE]; + int respondedp; + int re_xmitp, i; + fd_set lfdvar; + struct timeval timeout; + + if (fd == -1) { + fprintf(stderr, + "Trying again to reestablish connection with the robot...\n" + " "); + fflush(stderr); + for (i = 0; (i < 2) && + (connect_robot(Robot_id, model, device, conn_value) == FALSE); + i++) + { + sleep(5); + fprintf(stderr, "Trying again... "); + fflush(stderr); + } + if (i == 2 && + connect_robot(Robot_id, model, device, conn_value) == FALSE) + { + fprintf(stderr, "Failed to reestablish connection. Command aborted.\n"); + return FALSE; + } + fprintf(stderr, "Successful! Continuing with command.\n"); + } + re_xmitp = RE_XMIT; + FD_ZERO(&lfdvar); + FD_SET(fd, &lfdvar); + + timeout.tv_sec = 0; + timeout.tv_usec = 0; + + while (select(fd+1, &lfdvar, NULL, NULL, &timeout) != 0) + { + /* Flush buffer */ + respondedp = read(fd, response, BUFSIZE); + /* Check for errors, such as lost connection. */ + if (respondedp <= 0 && errno != EWOULDBLOCK) + { + close(fd); + Fd = -1; + fprintf(stderr, + "Lost communication with robot.\nAttempting to reconnect..."); + fflush(stderr); + for (i = 0; (i < 2) && + (connect_robot (Robot_id, model, device, conn_value) == FALSE); + i++) + { + sleep(5); + fprintf(stderr, "Trying again... "); + fflush(stderr); + } + if (i == 2 && + connect_robot (Robot_id, model, device, conn_value) == FALSE) + { + fprintf(stderr, "Unable to reconnect to robot. Command aborted.\n"); + return FALSE; + } + else + { + fprintf(stderr, "Successful! Continuing with command.\n"); + } + } + } + + Write_Pkg(fd, command); + while (!(respondedp = Read_Pkg(fd, connect_type, response)) && (RE_XMIT)) { + Write_Pkg(fd, command); + } + if (!respondedp) { + printf("Last command packet transmitted:\n"); + for (i = 0; i < command[1]+4; i++) + printf("%2.2x ", command[i]); + printf("\n"); + + return(FALSE); + } + else { + if (Process_Robot_Resp (response)) { + return(TRUE); + } + else { + printf("error in robot response\n"); + return(FALSE); + } + } +} + +/************************************ + * * + * Robot Serial Interface Functions * + * * + ************************************/ + +/* + * First some helper functions + */ +void stuff_length_type(int length, int ptype, unsigned char *byte_ptr); +void stuff_length_type(int length, int ptype, unsigned char *byte_ptr) +{ + byte_ptr++; /* skip the first byte of the buffer, which is + reserved for begin_pkg character */ + + unsigned_int_to_two_bytes(length, byte_ptr); + byte_ptr++; byte_ptr++; + *byte_ptr = ptype; +} + +void stuff_two_signed_int(int length, int ptype, int num1, int num2, + unsigned char *byte_ptr); +void stuff_two_signed_int(int length, int ptype, int num1, int num2, + unsigned char *byte_ptr) +{ + byte_ptr++; /* skip the first byte of the buffer, which is + reserved for begin_pkg character */ + + unsigned_int_to_two_bytes(length, byte_ptr); + byte_ptr++; byte_ptr++; + *byte_ptr = ptype; + byte_ptr++; + signed_int_to_two_bytes(num1, byte_ptr); + byte_ptr++; byte_ptr++; + signed_int_to_two_bytes(num2, byte_ptr); +} + +void stuff_three_unsigned_int(int length, int ptype, int num1, int num2, + int num3, unsigned char *byte_ptr); +void stuff_three_unsigned_int(int length, int ptype, int num1, int num2, + int num3, unsigned char *byte_ptr) +{ + byte_ptr++; /* skip the first byte of the buffer, which is + reserved for begin_pkg character */ + + unsigned_int_to_two_bytes(length, byte_ptr); + byte_ptr++; byte_ptr++; + *byte_ptr = ptype; + byte_ptr++; + unsigned_int_to_two_bytes(num1, byte_ptr); + byte_ptr++; byte_ptr++; + unsigned_int_to_two_bytes(num2, byte_ptr); + byte_ptr++; byte_ptr++; + unsigned_int_to_two_bytes(num3, byte_ptr); +} + +void stuff_three_signed_int(int length, int ptype, int num1, int num2, + int num3, unsigned char *byte_ptr); +void stuff_three_signed_int(int length, int ptype, int num1, int num2, + int num3, unsigned char *byte_ptr) +{ + byte_ptr++; /* skip the first byte of the buffer, which is + reserved for begin_pkg character */ + + unsigned_int_to_two_bytes(length, byte_ptr); + byte_ptr++; byte_ptr++; + *byte_ptr = ptype; + byte_ptr++; + signed_int_to_two_bytes(num1, byte_ptr); + byte_ptr++; byte_ptr++; + signed_int_to_two_bytes(num2, byte_ptr); + byte_ptr++; byte_ptr++; + signed_int_to_two_bytes(num3, byte_ptr); +} + +/*************** + * FUNCTION: posLongExtract + * PURPOSE: compose a long out of four bytes + * ARGUMENTS: unsigned char *inbuf : the pointer to the four bytes + * ALGORITHM: bit manipulation + * RETURN: long + * SIDE EFFECT: + * CALLS: + * CALLED BY: + ***************/ +static long posLongExtract( unsigned char *inbuf ) +{ + long tmp; + + tmp = (long) LONG_B(inbuf[3],inbuf[2],inbuf[1],inbuf[0]); + + if ( tmp & (1L << 31) ) + return ( -(tmp & ~(1L << 31) ) ); + else + return ( tmp ); +} + + +/*************** + * FUNCTION: posUnsignedLongExtract + * PURPOSE: compose an unsigned long out of four bytes + * ARGUMENTS: unsigned char *inbuf : the pointer to the four bytes + * ALGORITHM: bit manipulation + * RETURN: usigned long + * SIDE EFFECT: + * CALLS: + * CALLED BY: + ***************/ +static unsigned long posUnsignedLongExtract( unsigned char *inbuf ) +{ + return ( (unsigned long) LONG_B(inbuf[3],inbuf[2],inbuf[1],inbuf[0]) ); +} + + +/*************** + * FUNCTION: posPackageProcess + * PURPOSE: processes the part of the package with pos information + * ARGUMENTS: unsigned char *inbuf : pointer to the data in chars + * PosData *posData : this is were the posData are written to + * ALGORITHM: regroup the bytes and assign variables + * RETURN: int (the number of bytes read from the buffer) + * SIDE EFFECT: + * CALLS: + * CALLED BY: + ***************/ +static int posPackageProcess ( unsigned char *inbuf, PosData *posData ) +{ + int i = 0; + + /* copy the stuff from the buffer into the posData for the current robot */ + posData->config.configX = posLongExtract(inbuf + i++ * sizeof(long)); + posData->config.configY = posLongExtract(inbuf + i++ * sizeof(long)); + posData->config.configSteer = posLongExtract(inbuf + i++ * sizeof(long)); + posData->config.configTurret = posLongExtract(inbuf + i++ * sizeof(long)); + posData->config.velTrans = posLongExtract(inbuf + i++ * sizeof(long)); + posData->config.velSteer = posLongExtract(inbuf + i++ * sizeof(long)); + posData->config.velTurret = posLongExtract(inbuf + i++ * sizeof(long)); + posData->config.timeStamp = posUnsignedLongExtract(inbuf + i++ * + sizeof(long)); + posData->timeStamp = posUnsignedLongExtract(inbuf + i++ * + sizeof(long)); + + return ( i * sizeof(long) ); +} + + +/*************** + * FUNCTION: timePackageProcess + * PURPOSE: processes the part of the package with the 6811 time + * ARGUMENTS: unsigned char *inbuf : pointer to the data in chars + * unsigned long *time : this is were the time is written to + * ALGORITHM: --- + * RETURN: static int + * SIDE EFFECT: + * CALLS: + * CALLED BY: + ***************/ +static int timePackageProcess ( unsigned char *inbuf, unsigned long *timeS ) +{ + *timeS = posUnsignedLongExtract( inbuf ); + + return ( 4 ); +} + + +/*************** + * FUNCTION: voltPackageProcess + * PURPOSE: processes the part of the package with the voltages + * ARGUMENTS: unsigned char *inbuf : pointer to the data in chars + * unsigned long *time : this is were the time is written to + * ALGORITHM: --- + * RETURN: static int + * SIDE EFFECT: + * CALLS: + * CALLED BY: + ***************/ +static int voltPackageProcess ( unsigned char *inbuf, + unsigned char *voltCPU, + unsigned char *voltMotor) +{ + int i = 0; + + /* read the raw voltages out of the buffer */ + *voltCPU = *(inbuf + i++); + *voltMotor = *(inbuf + i++); + + return ( i ); +} + +/***************************** + * * + * Robot Interface Functions * + * * + *****************************/ + +/* + * dummy function to maintain compatibility with Nclient + * + * create_robot - requests the server to create a robot with + * id = robot_id and establishes a connection with + * the robot. This function is disabled in this + * version of the software. + * + * parameters: + * long robot_id -- id of the robot to be created. The robot + * will be referred to by this id. If a process + * wants to talk (connect) to a robot, it must + * know its id. + */ +int create_robot ( long robot_id ) +{ + Robot_id = robot_id; + return ( TRUE ); +} + +/* Helper function for connect_robot */ +static char *convertAddr ( char *name, char *addr ) +{ + int addrInt[10]; + + sscanf(name, "%d.%d.%d.%d", + &(addrInt[0]), &(addrInt[1]), &(addrInt[2]), &(addrInt[3])); + addr[0] = addrInt[0]; + addr[1] = addrInt[1]; + addr[2] = addrInt[2]; + addr[3] = addrInt[3]; + return ( addr ); +} + +int open_serial(char *port, unsigned short baud) +{ + struct termios info; + + if (Fd != -1) + close(Fd); + if ((Fd=open(port, O_RDWR|O_NONBLOCK)) < 0) + { + perror("Error opening serial port"); + return 0; + } + + if (tcgetattr(Fd, &info) < 0) + { + perror("Error using TCGETS in ioctl."); + close(Fd); + Fd = -1; + return 0; + } + + /* restore old values to unhang the bastard, if hung */ + info.c_iflag=1280; + info.c_oflag=5; + info.c_cflag=3261; + info.c_lflag=35387; + + if (tcsetattr(Fd, TCSANOW, &info) < 0) + { + perror("Error using TCSETS in ioctl."); + close(Fd); + Fd = -1; + return 0; + } + close(Fd); + + if ((Fd = open(port, O_RDWR)) == -1) { + perror("Error opening serial port"); + errorp = SERIAL_OPEN_ERROR; + return(FALSE); + } + + if (tcgetattr(Fd,&info) < 0) { + perror("Error using TCGETS in ioctl."); + errorp = SERIAL_OPEN_ERROR; + close(Fd); + Fd = -1; + return(FALSE); + } + + if (baud != 4800 && baud != 9600 && baud != 19200 && baud != 38400) + { + if (baud != 0) + { + fprintf(stderr, "Invalid baud rate %d, using %d\n", baud, + DEFAULT_SERIAL_BAUD); + } + baud = DEFAULT_SERIAL_BAUD; + } + + info.c_iflag = 0; + info.c_oflag = 0; + info.c_lflag = 0; + switch (baud) { /* serial port rate */ + case 4800: + info.c_cflag = B4800 | CS8 | CREAD | CLOCAL; + break; + case 9600: + info.c_cflag = B9600 | CS8 | CREAD | CLOCAL; + break; + case 19200: + info.c_cflag = B19200 | CS8 | CREAD | CLOCAL; + break; + case 38400: + info.c_cflag = B38400 | CS8 | CREAD | CLOCAL; + break; + default: + break; + } + /* set time out on serial read */ +#if 1 + info.c_cc[VMIN] = 0; + info.c_cc[VTIME] = 10; +#endif + wait_time = NORMAL_TIMEOUT; + + if (tcsetattr(Fd,TCSANOW,&info) < 0) { + perror("Error using TCSETS in ioctl."); + errorp = SERIAL_OPEN_ERROR; + close(Fd); + Fd = -1; + return(FALSE); + } + + printf("Robot <-> Host serial communication setup\n"); + printf("(%d baud using %s)\n", baud, port); + return(TRUE); /* ADDED BY DERYCK MORALES <de...@cm...> */ +} + +/* + * connect_robot - requests the server to connect to the robot + * with id = robot_id. In order to talk to the server, + * the SERVER_MACHINE_NAME and SERV_TCP_PORT must be + * set properly. If a robot with robot_id exists, + * a connection is established with that robot. If + * no robot exists with robot_id, no connection is + * established. In this single robot version, the robot_id + * is unimportant. You can call connect_robot with any + * robot_id, it will connect to the robot. + * + * parameters: + * long robot_id -- robot's id. In this multiple robot version, in order + * to connect to a robot, you must know it's id. + * model -- robot type: 0 = Nomad 200, 1 = Nomad 150, 2 = Scout + * *dev -- hostname for TCP, device file for serial ("/dev/" prefix + * or ":" suffix means serial) + * conn -- TCP port for TCP, baud rate for serial + */ +int connect_robot(long robot_id, ...) +{ + static char first = 1; + struct hostent *hp; + struct sockaddr_in serv_addr; + int ret, retlen, i; + unsigned char ir_mask[16],sn_mask[16],cf_mask[4],vl_mask[3]; + unsigned char cp_mask,bp_mask,ls_mask,pos_mask, byte; + char addr[10]; + + va_list args; + + if (first) + { + fprintf(stderr, "Ndirect version 2.6.13\n"); + fprintf(stderr, "Copyright 1991-1998, Nomadic Technologies, Inc.\n"); + first = 0; + } + + if(ROBOT_MODEL == MODEL_N200) return old_connect_robot(robot_id); + + va_start(args, robot_id); + model = va_arg(args, int); + device = va_arg(args, char *); + conn_value = va_arg(args, int); + if (strncmp(device, "/dev", 4) != 0 && device[strlen(device)-1] != ':') + { + connect_type = 2; + } + va_end(args); + + if (connect_type == 1) + { + open_serial(device, conn_value); + + /* Send init_sensors to make sure that server and robot are synchronized */ + if (model == MODEL_N200) + { + init_sensors(); + } + else + { + usedSmask[0] = 0; + /* IR */ + for (i = 1; i < 17; i++) + usedSmask[i] = 0; + /* Sonar */ + for (i = 17; i < 33; i++) + usedSmask[i] = 1; + /* Bumper */ + usedSmask[33] = 1; + /* Conf */ + for (i = 34; i < 38; i++) + usedSmask[i] = 1; + /* Velocity */ + for (i = 38; i < 41; i++) + usedSmask[i] = 1; + /* Motor */ + usedSmask[41] = 1; + /* Laser */ + usedSmask[42] = 0; + /* Compass */ + usedSmask[43] = 1; + } + } else { + if (device[0] == 0) + device = "localhost"; + if ( ((hp = gethostbyname(device)) == NULL)) + { + convertAddr(device, addr); + if (addr[0] != 0 || addr[1] != 0 || addr[2] != 0 || addr[3] != 0) + { + memset((char *) &serv_addr, 0, sizeof(serv_addr)); + memcpy((char *) &(serv_addr.sin_addr), addr, 4); + } + else + { + fprintf(stderr, "Machine %s not valid.\n", device); + return FALSE; + } + } + else + { + memset((char *) &serv_addr, 0, sizeof(serv_addr)); + memcpy((char *) &(serv_addr.sin_addr), hp->h_addr, hp->h_length); + } + + serv_addr.sin_family = AF_INET; /* address family */ + + /* TCP port number */ + if (conn_value == 0) + { + conn_value = DEFAULT_ROBOT_TCP_PORT; + } + serv_addr.sin_port = htons(conn_value); + + if ((Fd = socket(AF_INET, SOCK_STREAM, 0)) < 0) { + fprintf(stderr, "Error: in open_socket_to_send_data, socket failed.\n"); + return FALSE; + } + fcntl(Fd, F_SETFL, O_NDELAY); + if (connect(Fd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0) { + if (errno == EINPROGRESS) { + fd_set lfdvar; + struct timeval timeout; + + FD_ZERO(&lfdvar); + FD_SET(Fd, &lfdvar); + + timeout.tv_sec = (long)CONNECT_TIMEOUT; + timeout.tv_usec = (long)0; + + if (select(Fd+1, NULL, &lfdvar, NULL, &timeout) == 0) { + fprintf(stderr, "Error: connect timed out.\n"); + close(Fd); + Fd = -1; + return FALSE; + } else { + errno = 0; + retlen = 4; + if (getsockopt(Fd, SOL_SOCKET, SO_ERROR, (char *)&ret, &retlen) == 0) + { + if (ret != 0) + errno = ret; + if (errno != 0) + { + perror("Error: connect failed"); + close(Fd); + Fd = -1; + return FALSE; + } + } + } + } else { + perror("Error: connect failed"); + close(Fd); + Fd = -1; + return FALSE; + } + } + + wait_time = NORMAL_TIMEOUT; + + printf("Robot <-> Host TCP/IP communication setup\n"); + printf("(machine %s on port %d)\n", device, conn_value); + + /* Read configuration data */ + if (model == MODEL_N200) + { + byte = GETC(Fd, connect_type); + ir_mask[ 0] = byte & (1 << 0) ? 1 : 0; + ir_mask[ 1] = byte & (1 << 1) ? 1 : 0; + ir_mask[ 2] = byte & (1 << 2) ? 1 : 0; + ir_mask[ 3] = byte & (1 << 3) ? 1 : 0; + ir_mask[ 4] = byte & (1 << 4) ? 1 : 0; + ir_mask[ 5] = byte & (1 << 5) ? 1 : 0; + ir_mask[ 6] = byte & (1 << 6) ? 1 : 0; + ir_mask[ 7] = byte & (1 << 7) ? 1 : 0; + byte = GETC(Fd, connect_type); + ir_mask[ 8] = byte & (1 << 0) ? 1 : 0; + ir_mask[ 9] = byte & (1 << 1) ? 1 : 0; + ir_mask[10] = byte & (1 << 2) ? 1 : 0; + ir_mask[11] = byte & (1 << 3) ? 1 : 0; + ir_mask[12] = byte & (1 << 4) ? 1 : 0; + ir_mask[13] = byte & (1 << 5) ? 1 : 0; + ir_mask[14] = byte & (1 << 6) ? 1 : 0; + ir_mask[15] = byte & (1 << 7) ? 1 : 0; + byte = GETC(Fd, connect_type); + sn_mask[ 0] = byte & (1 << 0) ? 1 : 0; + sn_mask[ 1] = byte & (1 << 1) ? 1 : 0; + sn_mask[ 2] = byte & (1 << 2) ? 1 : 0; + sn_mask[ 3] = byte & (1 << 3) ? 1 : 0; + sn_mask[ 4] = byte & (1 << 4) ? 1 : 0; + sn_mask[ 5] = byte & (1 << 5) ? 1 : 0; + sn_mask[ 6] = byte & (1 << 6) ? 1 : 0; + sn_mask[ 7] = byte & (1 << 7) ? 1 : 0; + byte = GETC(Fd, connect_type); +... [truncated message content] |
From: <gb...@us...> - 2008-10-02 00:44:24
|
Revision: 7057 http://playerstage.svn.sourceforge.net/playerstage/?rev=7057&view=rev Author: gbiggs Date: 2008-10-02 00:44:06 +0000 (Thu, 02 Oct 2008) Log Message: ----------- Removed erroneous HAVE_CONFIG_H checks, deleted old hand-coded driverregistry.cc Modified Paths: -------------- code/player/trunk/examples/plugins/logplugin/logplugin.cc code/player/trunk/server/drivers/gps/garminnmea.cc code/player/trunk/server/drivers/laser/sicklms200.cc code/player/trunk/server/drivers/localization/amcl/amcl.cc code/player/trunk/server/drivers/localization/amcl/amcl_fiducial.cc code/player/trunk/server/drivers/localization/amcl/amcl_gps.cc code/player/trunk/server/drivers/localization/amcl/amcl_imu.cc code/player/trunk/server/drivers/localization/amcl/amcl_laser.cc code/player/trunk/server/drivers/localization/amcl/amcl_odom.cc code/player/trunk/server/drivers/localization/amcl/amcl_sensor.cc code/player/trunk/server/drivers/localization/amcl/amcl_sonar.cc code/player/trunk/server/drivers/localization/amcl/amcl_wifi.cc code/player/trunk/server/drivers/localization/amcl/map/map_draw.c 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/mixed/botrics/obot.cc code/player/trunk/server/drivers/mixed/erratic/erratic.cc code/player/trunk/server/drivers/mixed/irobot/create/create_comms.c code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_comms.c 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/wbr/914/wbr914.cc code/player/trunk/server/drivers/position/microstrain/3dmg.cc code/player/trunk/server/drivers/position/nav200/sicknav200.cc code/player/trunk/server/drivers/ptz/amtecpowercube.cc code/player/trunk/server/drivers/ptz/canonvcc4.cc code/player/trunk/server/drivers/ptz/sonyevid30.cc code/player/trunk/utils/playercam/playercam.c Removed Paths: ------------- code/player/trunk/server/libplayerdrivers/driverregistry.cc Modified: code/player/trunk/examples/plugins/logplugin/logplugin.cc =================================================================== --- code/player/trunk/examples/plugins/logplugin/logplugin.cc 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/examples/plugins/logplugin/logplugin.cc 2008-10-02 00:44:06 UTC (rev 7057) @@ -1,9 +1,9 @@ /* * 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 @@ -25,12 +25,10 @@ * shared object. */ -// ONLY if you need something that was #define'd as a result of configure +// ONLY if you need something that was #define'd as a result of configure // (e.g., HAVE_CFMAKERAW), then #include <config.h>, like so: /* -#if HAVE_CONFIG_H - #include <config.h> -#endif +#include <config.h> */ #include <unistd.h> @@ -62,7 +60,7 @@ va_end(ap); fprintf(msgFile, "New error print: %s:%d %s", file, line, msgBuf); } - + return; } Modified: code/player/trunk/server/drivers/gps/garminnmea.cc =================================================================== --- code/player/trunk/server/drivers/gps/garminnmea.cc 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/gps/garminnmea.cc 2008-10-02 00:44:06 UTC (rev 7057) @@ -91,9 +91,7 @@ */ /** @} */ -#ifdef HAVE_CONFIG_H - #include "config.h" -#endif +#include "config.h" #include <fcntl.h> Modified: code/player/trunk/server/drivers/laser/sicklms200.cc =================================================================== --- code/player/trunk/server/drivers/laser/sicklms200.cc 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/laser/sicklms200.cc 2008-10-02 00:44:06 UTC (rev 7057) @@ -182,9 +182,7 @@ -#if HAVE_CONFIG_H - #include <config.h> -#endif +#include <config.h> #include <assert.h> #include <math.h> Modified: code/player/trunk/server/drivers/localization/amcl/amcl.cc =================================================================== --- code/player/trunk/server/drivers/localization/amcl/amcl.cc 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/localization/amcl/amcl.cc 2008-10-02 00:44:06 UTC (rev 7057) @@ -276,9 +276,7 @@ /** @} */ -#ifdef HAVE_CONFIG_H #include "config.h" -#endif #include <assert.h> #include <errno.h> Modified: code/player/trunk/server/drivers/localization/amcl/amcl_fiducial.cc =================================================================== --- code/player/trunk/server/drivers/localization/amcl/amcl_fiducial.cc 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/localization/amcl/amcl_fiducial.cc 2008-10-02 00:44:06 UTC (rev 7057) @@ -27,9 +27,7 @@ // /////////////////////////////////////////////////////////////////////////// -#ifdef HAVE_CONFIG_H #include "config.h" -#endif #define PLAYER_ENABLE_MSG 1 @@ -52,7 +50,7 @@ { this->driver = NULL; this->fiducial_id = id; - + return; } @@ -70,7 +68,7 @@ // SetupMap(). cf->ReadDeviceId(&(this->map_id), section, "requires", PLAYER_MAP_CODE, -1, "fiducial"); - + // Get the map settings map_filename = (char*) cf->ReadFilename(section, "fiducial_map", NULL); if( read_map_file( map_filename ) < 0 ) @@ -78,7 +76,7 @@ PLAYER_ERROR( "read_map_file failed" ); return -1; } - + this->laser_pose.v[0] = cf->ReadTupleLength(section, "laser_pose", 0, 0); this->laser_pose.v[1] = cf->ReadTupleLength(section, "laser_pose", 1, 0); this->laser_pose.v[2] = cf->ReadTupleAngle(section, "laser_pose", 2, 0); @@ -119,7 +117,7 @@ for( int i = 0; i < x; i++ ) { int x, y, id; - + fscanf( map_file, "%d", &id); this->fmap->fiducials[i][2] = (double) id; fscanf( map_file, "%d", &(x )); @@ -137,7 +135,7 @@ { //laser_free(this->model); //this->model = NULL; - + return 0; } @@ -194,8 +192,8 @@ //if(map_load_occ(this->map, map_filename, map_scale, map_negate) != 0) //return -1; - // Fill in the map structure (I'm doing it here instead of in libmap, - // because libmap is written in C, so it'd be a pain to invoke the internal + // Fill in the map structure (I'm doing it here instead of in libmap, + // because libmap is written in C, so it'd be a pain to invoke the internal // device API from there) // first, get the map info @@ -204,14 +202,14 @@ player_map_info_t info; struct timeval ts; info.subtype = PLAYER_MAP_REQ_GET_INFO; - if((replen = mapdriver->Request(this->map_id, this, + if((replen = mapdriver->Request(this->map_id, this, &info, sizeof(info.subtype), NULL, &reptype, &info, sizeof(info), &ts)) == 0) { PLAYER_ERROR("failed to get map info"); return(-1); } - + // copy in the map info this->map->origin_x = this->map->origin_y = 0.0; this->map->scale = 1/(ntohl(info.scale) / 1e3); @@ -232,7 +230,7 @@ int si,sj; data_req.subtype = PLAYER_MAP_REQ_GET_DATA; - + // Tile size sy = sx = (int)sqrt(sizeof(data_req.data)); assert(sx * sy < (int)sizeof(data_req.data)); @@ -249,9 +247,9 @@ reqlen = sizeof(data_req) - sizeof(data_req.data); - if((replen = mapdriver->Request(this->map_id, this, + if((replen = mapdriver->Request(this->map_id, this, &data_req, reqlen, NULL, - &reptype, + &reptype, &data_req, sizeof(data_req), &ts)) == 0) { PLAYER_ERROR("failed to get map info"); @@ -269,7 +267,7 @@ { for(i=0;i<si;i++) { - this->map->cells[MAP_INDEX(this->map,oi+i,oj+j)].occ_state = + this->map->cells[MAP_INDEX(this->map,oi+i,oj+j)].occ_state = data_req.data[j*si + i]; this->map->cells[MAP_INDEX(this->map,oi+i,oj+j)].occ_dist = 0; } @@ -294,7 +292,7 @@ //////////////////////////////////////////////////////////////////////////////// // Shut down the laser int AMCLFiducial::Shutdown(void) -{ +{ this->driver->Unsubscribe(this->fiducial_id); this->driver = NULL; map_free(this->map); @@ -317,27 +315,27 @@ AMCLFiducialData *ndata; // Get the laser device data. - size = this->driver->GetData(this->fiducial_id, (void*) &data, + size = this->driver->GetData(this->fiducial_id, (void*) &data, sizeof(data), ×tamp); if (size == 0) return NULL; - if((timestamp.tv_sec == this->time.tv_sec) && + if((timestamp.tv_sec == this->time.tv_sec) && (timestamp.tv_usec == this->time.tv_usec)) return NULL; double ta = (double) timestamp.tv_sec + ((double) timestamp.tv_usec) * 1e-6; - double tb = (double) this->time.tv_sec + ((double) this->time.tv_usec) * 1e-6; + double tb = (double) this->time.tv_sec + ((double) this->time.tv_usec) * 1e-6; if (ta - tb < 0.100) // HACK return NULL; this->time = timestamp; - + ndata = new AMCLFiducialData; ndata->sensor = this; ndata->tsec = timestamp.tv_sec; ndata->tusec = timestamp.tv_usec; - + ndata->fiducial_count = ntohs(data.count); assert((size_t) ndata->fiducial_count < sizeof(ndata->fiducials) / sizeof(ndata->fiducials[0])); @@ -346,15 +344,15 @@ { double x = ((int16_t) ntohl(data.fiducials[i].pos[0]))/ 1000.0; double y = ((int16_t) ntohl(data.fiducials[i].pos[1]))/ 1000.0; - + r = hypot( y, x ); b = atan2( y, x ); - + ndata->fiducials[i][0] = r; ndata->fiducials[i][1] = b; ndata->fiducials[i][2] = ((double) ntohs( data.fiducials[i].id )); } - + return ndata; } @@ -364,12 +362,12 @@ bool AMCLFiducial::UpdateSensor(pf_t *pf, AMCLSensorData *data) { AMCLFiducialData *ndata; - + ndata = (AMCLFiducialData*) data; // Apply the laser sensor model pf_update_sensor(pf, (pf_sensor_model_fn_t) SensorModel, data); - + return true; } @@ -386,7 +384,7 @@ double map_range, map_bearing; double obs_range, obs_bearing; int obs_id; - + self = (AMCLFiducial*) data->sensor; // Take account of the laser pose relative to the robot @@ -401,7 +399,7 @@ obs_id = (int) data->fiducials[i][2]; int id = -1; - + for( int j = 0; j < self->fmap->fiducial_count; j++ ) { if( (int) data->fiducials[i][2] == (int) self->fmap->fiducials[j][2] ) @@ -409,7 +407,7 @@ id = j; } } - + if( id > -1 ) { @@ -429,27 +427,27 @@ c = self->range_var; z = obs_range - map_range; pz = self->range_bad + (1 - self->range_bad) * exp(-(z*z)/(2*c*c)); - + v = self->angle_var; b = obs_bearing - map_bearing; pb = self->angle_bad + (1 - self->angle_bad) * exp(-(b*b)/(2*v*v)); - + } //gives the fiducial range greater weight p *= pz; - + //bearing a little less p *= pb; p *= pb; p *= pb; p *= pb; - + } } //printf("%e\n", p); //assert(p >= 0); - + return p; } @@ -460,16 +458,16 @@ //////////////////////////////////////////////////////////////////////////////// // Setup the GUI void AMCLFiducial::SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig) -{ +{ this->fig = rtk_fig_create(canvas, robot_fig, 0); // Draw the laser map this->map_fig = rtk_fig_create(canvas, NULL, -50); map_draw_occ(this->map, this->map_fig); - + char x[3]; x[2] = '\0'; - + rtk_fig_color_rgb32( map_fig, 0xFF00FF ); for( int i = 0; i < fmap->fiducial_count; i++ ) { @@ -477,9 +475,9 @@ x[1] = '0' + (char) ((int) fmap->fiducials[i][2] % 10); rtk_fig_ellipse(this->map_fig, fmap->fiducials[i][0], fmap->fiducials[i][1], 0, 0.1, 0.1, 1 ); rtk_fig_text( this->map_fig, fmap->fiducials[i][0], fmap->fiducials[i][1]+0.2, 0, x ); - + } - + return; } @@ -506,7 +504,7 @@ AMCLFiducialData *ndata; ndata = (AMCLFiducialData*) data; - + rtk_fig_clear(this->fig); // Draw the complete scan @@ -553,12 +551,12 @@ AMCLFiducialMap* map; map = new AMCLFiducialMap(); map->fiducial_count = 0; - + if( !map ) { PLAYER_ERROR( "map is undefined" ); } - + return map; } @@ -575,7 +573,7 @@ { b += 2*M_PI; } - + return b; } #endif Modified: code/player/trunk/server/drivers/localization/amcl/amcl_gps.cc =================================================================== --- code/player/trunk/server/drivers/localization/amcl/amcl_gps.cc 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/localization/amcl/amcl_gps.cc 2008-10-02 00:44:06 UTC (rev 7057) @@ -27,9 +27,7 @@ // /////////////////////////////////////////////////////////////////////////// -#ifdef HAVE_CONFIG_H #include "config.h" -#endif #include "devicetable.h" #include "amcl_gps.h" @@ -41,7 +39,7 @@ { this->device = NULL; this->model = NULL; - + return; } @@ -52,7 +50,7 @@ { // Device stuff this->gps_index = cf->ReadInt(section, "gps_index", -1); - + // Create the gps model this->model = gps_alloc(); this->model->utm_base_e = cf->ReadTupleFloat(section, "utm_base", 0, -1); @@ -68,7 +66,7 @@ { gps_free(this->model); this->model = NULL; - + return 0; } @@ -102,7 +100,7 @@ //////////////////////////////////////////////////////////////////////////////// // Shut down the gps int AMCLGps::Shutdown(void) -{ +{ this->device->Unsubscribe(this); this->device = NULL; @@ -132,7 +130,7 @@ this->utm_e = ((int32_t) ntohl(data.utm_e)) / 100.0; this->utm_n = ((int32_t) ntohl(data.utm_n)) / 100.0; this->err_horz = ((int32_t) ntohl(data.err_horz)) / 1000.0; - + return true; } @@ -145,7 +143,7 @@ if (!this->GetData()) return false; - printf("init gps %f %f %f\n", + printf("init gps %f %f %f\n", this->utm_e, this->utm_n, this->err_horz); // Pick up UTM base coordinate @@ -162,12 +160,12 @@ // Initialize the initialization routines gps_init_init(this->model); - + // Draw samples from the gps distribution pf_init(pf, (pf_init_model_fn_t) gps_init_model, this->model); - + gps_init_term(this->model); - + return true; } @@ -179,15 +177,15 @@ // Check for new data if (!this->GetData()) return false; - - printf("update gps %f %f %f\n", + + printf("update gps %f %f %f\n", this->utm_e, this->utm_n, this->err_horz); - + // Update the gps sensor model with the latest gps measurements gps_set_utm(this->model, this->utm_e, this->utm_n, this->err_horz); // Apply the gps sensor model - pf_update_sensor(pf, (pf_sensor_model_fn_t) gps_sensor_model, this->model); + pf_update_sensor(pf, (pf_sensor_model_fn_t) gps_sensor_model, this->model); return true; } @@ -199,7 +197,7 @@ void AMCLGps::SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig) { this->fig = rtk_fig_create(canvas, NULL, 0); - + return; } @@ -225,7 +223,7 @@ this->utm_e - this->model->utm_base_e, this->utm_n - this->model->utm_base_n, 0, this->err_horz, this->err_horz, 0); - + return; } Modified: code/player/trunk/server/drivers/localization/amcl/amcl_imu.cc =================================================================== --- code/player/trunk/server/drivers/localization/amcl/amcl_imu.cc 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/localization/amcl/amcl_imu.cc 2008-10-02 00:44:06 UTC (rev 7057) @@ -27,9 +27,7 @@ // /////////////////////////////////////////////////////////////////////////// -#ifdef HAVE_CONFIG_H #include "config.h" -#endif #include "devicetable.h" #include "amcl_imu.h" @@ -41,7 +39,7 @@ { this->device = NULL; this->model = NULL; - + return; } @@ -52,7 +50,7 @@ { // Device stuff this->imu_index = cf->ReadInt(section, "imu_index", -1); - + // Create the imu model this->model = imu_alloc(); @@ -72,7 +70,7 @@ { imu_free(this->model); this->model = NULL; - + return 0; } @@ -106,7 +104,7 @@ //////////////////////////////////////////////////////////////////////////////// // Shut down the imu int AMCLImu::Shutdown(void) -{ +{ this->device->Unsubscribe(this); this->device = NULL; @@ -135,7 +133,7 @@ this->utm_head = ((int32_t) ntohl(data.yaw)) / 3600.0 * M_PI / 180; this->utm_head += this->utm_mag_dev; - + return true; } @@ -147,14 +145,14 @@ // Check for new data if (!this->GetData()) return false; - + printf("update imu %f\n", this->utm_head * 180 / M_PI); - + // Update the imu sensor model with the latest imu measurements imu_set_utm(this->model, this->utm_head); // Apply the imu sensor model - pf_update_sensor(pf, (pf_sensor_model_fn_t) imu_sensor_model, this->model); + pf_update_sensor(pf, (pf_sensor_model_fn_t) imu_sensor_model, this->model); return true; } @@ -166,7 +164,7 @@ void AMCLImu::SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig) { this->fig = rtk_fig_create(canvas, NULL, 0); - + return; } @@ -177,7 +175,7 @@ { rtk_fig_destroy(this->fig); this->fig = NULL; - + return; } @@ -186,12 +184,12 @@ void AMCLImu::UpdateGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig) { double ox, oy, oa; - + rtk_fig_clear(this->fig); rtk_fig_color_rgb32(this->fig, 0xFF00FF); rtk_fig_get_origin(robot_fig, &ox, &oy, &oa); rtk_fig_arrow(this->fig, ox, oy, this->utm_head + M_PI / 2, 1.0, 0.20); - + return; } Modified: code/player/trunk/server/drivers/localization/amcl/amcl_laser.cc =================================================================== --- code/player/trunk/server/drivers/localization/amcl/amcl_laser.cc 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/localization/amcl/amcl_laser.cc 2008-10-02 00:44:06 UTC (rev 7057) @@ -27,9 +27,7 @@ // /////////////////////////////////////////////////////////////////////////// -#ifdef HAVE_CONFIG_H #include "config.h" -#endif #define PLAYER_ENABLE_MSG 1 @@ -48,7 +46,7 @@ { this->laser_dev = NULL; this->laser_addr = addr; - + return; } @@ -61,7 +59,7 @@ // SetupMap(). cf->ReadDeviceAddr(&(this->map_addr), section, "requires", PLAYER_MAP_CODE, -1, "laser"); - + this->max_beams = cf->ReadInt(section, "laser_max_beams", 6); this->range_var = cf->ReadLength(section, "laser_range_var", 0.10); this->range_bad = cf->ReadFloat(section, "laser_range_bad", 0.10); @@ -78,7 +76,7 @@ { //laser_free(this->model); //this->model = NULL; - + return 0; } @@ -156,8 +154,8 @@ this->map = map_alloc(); PLAYER_MSG1(2, "AMCL loading map from map:%d...", this->map_addr.index); - // Fill in the map structure (I'm doing it here instead of in libmap, - // because libmap is written in C, so it'd be a pain to invoke the internal + // Fill in the map structure (I'm doing it here instead of in libmap, + // because libmap is written in C, so it'd be a pain to invoke the internal // device API from there) // first, get the map info @@ -174,7 +172,7 @@ PLAYER_MSG1(2, "AMCL loading map from map:%d...Done", this->map_addr.index); player_map_info_t* info = (player_map_info_t*)msg->GetPayload(); - + // copy in the map info this->map->origin_x = info->origin.px + (info->scale * info->width) / 2.0; this->map->origin_y = info->origin.py + (info->scale * info->height) / 2.0; @@ -232,7 +230,7 @@ { for(i=0;i<si;i++) { - this->map->cells[MAP_INDEX(this->map,oi+i,oj+j)].occ_state = + this->map->cells[MAP_INDEX(this->map,oi+i,oj+j)].occ_state = mapdata->data[j*si + i]; this->map->cells[MAP_INDEX(this->map,oi+i,oj+j)].occ_dist = 0; } @@ -263,7 +261,7 @@ //////////////////////////////////////////////////////////////////////////////// // Shut down the laser int AMCLLaser::Shutdown(void) -{ +{ this->laser_dev->Unsubscribe(AMCL.InQueue); this->laser_dev = NULL; map_free(this->map); @@ -276,8 +274,8 @@ // Get the current laser reading //AMCLSensorData *AMCLLaser::GetData(void) // Process message for this interface -int AMCLLaser::ProcessMessage(QueuePointer &resp_queue, - player_msghdr * hdr, +int AMCLLaser::ProcessMessage(QueuePointer &resp_queue, + player_msghdr * hdr, void * idata) { int i; @@ -290,15 +288,15 @@ this->time = hdr->timestamp; player_laser_data_t* data = reinterpret_cast<player_laser_data_t*> (idata); - + b = data->min_angle; db = data->resolution; - + ndata = new AMCLLaserData; ndata->sensor = this; ndata->time = hdr->timestamp; - + ndata->range_count = data->ranges_count; ndata->range_max = data->max_range; ndata->ranges = new double [ndata->range_count][2]; @@ -313,7 +311,7 @@ } AMCL.Push(ndata); - + return 0; } @@ -323,14 +321,14 @@ bool AMCLLaser::UpdateSensor(pf_t *pf, AMCLSensorData *data) { AMCLLaserData *ndata; - + ndata = (AMCLLaserData*) data; if (this->max_beams < 2) return false; // Apply the laser sensor model pf_update_sensor(pf, (pf_sensor_model_fn_t) SensorModel, data); - + return true; } @@ -348,11 +346,11 @@ double total_weight; pf_sample_t *sample; pf_vector_t pose; - + self = (AMCLLaser*) data->sensor; total_weight = 0.0; - + // Compute the sample weights for (j = 0; j < set->sample_count; j++) { @@ -418,13 +416,13 @@ //////////////////////////////////////////////////////////////////////////////// // Setup the GUI void AMCLLaser::SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig) -{ +{ this->fig = rtk_fig_create(canvas, robot_fig, 0); // Draw the laser map this->map_fig = rtk_fig_create(canvas, NULL, -50); map_draw_occ(this->map, this->map_fig); - + return; } @@ -451,7 +449,7 @@ AMCLLaserData *ndata; ndata = (AMCLLaserData*) data; - + rtk_fig_clear(this->fig); // Draw the complete scan Modified: code/player/trunk/server/drivers/localization/amcl/amcl_odom.cc =================================================================== --- code/player/trunk/server/drivers/localization/amcl/amcl_odom.cc 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/localization/amcl/amcl_odom.cc 2008-10-02 00:44:06 UTC (rev 7057) @@ -27,9 +27,7 @@ // /////////////////////////////////////////////////////////////////////////// -#ifdef HAVE_CONFIG_H #include "config.h" -#endif #include <sys/types.h> // required by Darwin #include <math.h> @@ -45,7 +43,7 @@ this->odom_dev = NULL; this->action_pdf = NULL; this->odom_addr = addr; - + return; } @@ -60,7 +58,7 @@ this->drift.m[0][0] = cf->ReadTupleFloat(section, "odom_drift[0]", 0, 0.20); this->drift.m[0][1] = cf->ReadTupleFloat(section, "odom_drift[0]", 1, 0.00); - this->drift.m[0][2] = cf->ReadTupleFloat(section, "odom_drift[0]", 2, 0.00); + this->drift.m[0][2] = cf->ReadTupleFloat(section, "odom_drift[0]", 2, 0.00); this->drift.m[1][0] = cf->ReadTupleFloat(section, "odom_drift[1]", 0, 0.00); this->drift.m[1][1] = cf->ReadTupleFloat(section, "odom_drift[1]", 1, 0.20); @@ -77,7 +75,7 @@ //////////////////////////////////////////////////////////////////////////////// // Unload the model int AMCLOdom::Unload(void) -{ +{ return 0; } @@ -98,7 +96,7 @@ PLAYER_ERROR("unable to subscribe to position device"); return -1; } - + return 0; } @@ -110,7 +108,7 @@ // Unsubscribe from device this->odom_dev->Unsubscribe(AMCL.InQueue); this->odom_dev = NULL; - + return 0; } @@ -119,8 +117,8 @@ // Get the current odometry reading //AMCLSensorData *AMCLOdom::GetData(void) // Process message for this interface -int AMCLOdom::ProcessMessage(QueuePointer &resp_queue, - player_msghdr * hdr, +int AMCLOdom::ProcessMessage(QueuePointer &resp_queue, + player_msghdr * hdr, void * idata) { pf_vector_t pose; @@ -139,7 +137,7 @@ pose.v[1] = data->pos.py; pose.v[2] = data->pos.pa; - //printf("getdata %.3f %.3f %.3f\n", + //printf("getdata %.3f %.3f %.3f\n", //pose.v[0], pose.v[1], pose.v[2]); ndata = new AMCLOdomData; @@ -153,7 +151,7 @@ this->time = hdr->timestamp; AMCL.Push(ndata); - + return 0; } @@ -173,10 +171,10 @@ ndata->pose.v[0], ndata->pose.v[1], ndata->pose.v[2], ndata->delta.v[0], ndata->delta.v[1], ndata->delta.v[2]); */ - + // See how far the robot has moved x = ndata->delta; - + // Odometric drift model // This could probably be improved ux = this->drift.m[0][0] * x.v[0]; @@ -191,24 +189,24 @@ cx.m[2][2] = ua * ua; //printf("x = %f %f %f\n", x.v[0], x.v[1], x.v[2]); - + // Create a pdf with suitable characterisitics - this->action_pdf = pf_pdf_gaussian_alloc(x, cx); + this->action_pdf = pf_pdf_gaussian_alloc(x, cx); // Update the filter - pf_update_action(pf, (pf_action_model_fn_t) ActionModel, this); + pf_update_action(pf, (pf_action_model_fn_t) ActionModel, this); // Delete the pdf pf_pdf_gaussian_free(this->action_pdf); this->action_pdf = NULL; - + return true; } //////////////////////////////////////////////////////////////////////////////// // The action model function (static method) -void +void AMCLOdom::ActionModel(AMCLOdom *self, pf_sample_set_t* set) { int i; Modified: code/player/trunk/server/drivers/localization/amcl/amcl_sensor.cc =================================================================== --- code/player/trunk/server/drivers/localization/amcl/amcl_sensor.cc 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/localization/amcl/amcl_sensor.cc 2008-10-02 00:44:06 UTC (rev 7057) @@ -20,16 +20,14 @@ */ /////////////////////////////////////////////////////////////////////////// // -// Desc: AMCL sensor +// Desc: AMCL sensor // Author: Andrew Howard // Date: 6 Feb 2003 // CVS: $Id$ // /////////////////////////////////////////////////////////////////////////// -#ifdef HAVE_CONFIG_H #include "config.h" -#endif #include "amcl_sensor.h" @@ -57,7 +55,7 @@ //////////////////////////////////////////////////////////////////////////////// // Unload the model int AMCLSensor::Unload(void) -{ +{ return 0; } Modified: code/player/trunk/server/drivers/localization/amcl/amcl_sonar.cc =================================================================== --- code/player/trunk/server/drivers/localization/amcl/amcl_sonar.cc 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/localization/amcl/amcl_sonar.cc 2008-10-02 00:44:06 UTC (rev 7057) @@ -27,9 +27,7 @@ // /////////////////////////////////////////////////////////////////////////// -#ifdef HAVE_CONFIG_H #include "config.h" -#endif #include <math.h> #include "amcl.h" @@ -62,7 +60,7 @@ // If there is no sonar device... if (this->sonar_index < 0) return 0; - + id.code = PLAYER_SONAR_CODE; id.index = this->sonar_index; @@ -89,7 +87,7 @@ this->sonar_pose_count = (int16_t) ntohs(geom.pose_count); assert((size_t) this->sonar_pose_count < sizeof(this->sonar_poses) / sizeof(this->sonar_poses[0])); - + for (i = 0; i < this->sonar_pose_count; i++) { this->sonar_poses[i].v[0] = ((int16_t) ntohs(geom.poses[i][0])) / 1000.0; @@ -139,7 +137,7 @@ data->sonar_range_count = 0; return; } - + // Get the sonar device data. size = this->sonar->GetData(this, (uint8_t*) &ndata, sizeof(ndata), NULL, NULL); @@ -163,7 +161,7 @@ bool AdaptiveMCL::UpdateSonarModel(amcl_sensor_data_t *data) { int i; - + // If there is no sonar device... if (this->sonar_index < 0) return false; @@ -188,14 +186,14 @@ { int i; double r, b, ax, ay, bx, by; - + // If there is no sonar device... if (this->sonar_index < 0) return; rtk_fig_clear(this->sonar_fig); rtk_fig_color_rgb32(this->sonar_fig, 0xC0C080); - + for (i = 0; i < data->sonar_range_count; i++) { r = data->sonar_ranges[i]; @@ -206,7 +204,7 @@ bx = ax + r * cos(b); by = ay + r * sin(b); - + rtk_fig_line(this->sonar_fig, ax, ay, bx, by); } return; Modified: code/player/trunk/server/drivers/localization/amcl/amcl_wifi.cc =================================================================== --- code/player/trunk/server/drivers/localization/amcl/amcl_wifi.cc 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/localization/amcl/amcl_wifi.cc 2008-10-02 00:44:06 UTC (rev 7057) @@ -27,9 +27,7 @@ // /////////////////////////////////////////////////////////////////////////// -#ifdef HAVE_CONFIG_H #include "config.h" -#endif #include "amcl.h" @@ -103,14 +101,14 @@ // If there is no wifi device... if (this->wifi_index < 0) return 0; - + this->wifi->Unsubscribe(this); this->wifi = NULL; // Delete the wifi model wifi_free(this->wifi_model); this->wifi_model = NULL; - + return 0; } @@ -124,7 +122,7 @@ player_wifi_data_t ndata; player_wifi_link_t *link; amcl_wifi_beacon_t *beacon; - + // If there is no wifi device... if (this->wifi_index < 0) { @@ -136,7 +134,7 @@ size = this->wifi->GetData(this, (uint8_t*) &ndata, sizeof(ndata), NULL, NULL); data->wifi_level_count = this->wifi_beacon_count; - + for (i = 0; i < this->wifi_beacon_count; i++) { beacon = this->wifi_beacons + i; @@ -145,7 +143,7 @@ for (j = 0; j < ntohs(ndata.link_count); j++) { link = ndata.links + j; - + if (strcmp(link->ip, beacon->hostname) == 0) { data->wifi_levels[i] = (int16_t) ntohs(link->level); @@ -154,7 +152,7 @@ } } } - + return; } @@ -166,12 +164,12 @@ // If there is no wifi device... if (this->wifi_index < 0) return false; - + // Update the wifi sensor model with the latest wifi measurements wifi_set_levels(this->wifi_model, data->wifi_level_count, data->wifi_levels); // Apply the wifi sensor model - pf_update_sensor(this->pf, (pf_sensor_model_fn_t) wifi_sensor_model, this->wifi_model); + pf_update_sensor(this->pf, (pf_sensor_model_fn_t) wifi_sensor_model, this->wifi_model); return true; } @@ -200,14 +198,14 @@ // Get the cell at this pose cell = map_get_cell(this->map, pose.v[0], pose.v[1], pose.v[2]); - + text[0] = 0; for (i = 0; i < data->wifi_level_count; i++) { hostname = this->wifi_beacons[i].hostname; olevel = data->wifi_levels[i]; mlevel = cell->wifi_levels[i]; - + snprintf(ntext, sizeof(ntext), "%s %02d [%02d]\n", hostname, olevel, mlevel); strcat(text, ntext); } @@ -215,7 +213,7 @@ rtk_fig_clear(this->wifi_fig); rtk_fig_color_rgb32(this->wifi_fig, 0xFFFF00); rtk_fig_text(this->wifi_fig, +1, +1, 0, text); - + return; } Modified: code/player/trunk/server/drivers/localization/amcl/map/map_draw.c =================================================================== --- code/player/trunk/server/drivers/localization/amcl/map/map_draw.c 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/localization/amcl/map/map_draw.c 2008-10-02 00:44:06 UTC (rev 7057) @@ -6,9 +6,7 @@ * CVS: $Id$ **************************************************************************/ -#ifdef HAVE_CONFIG_H #include "config.h" -#endif #include <playerconfig.h> Modified: code/player/trunk/server/drivers/localization/amcl/pf/pf_draw.c =================================================================== --- code/player/trunk/server/drivers/localization/amcl/pf/pf_draw.c 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/localization/amcl/pf/pf_draw.c 2008-10-02 00:44:06 UTC (rev 7057) @@ -6,9 +6,7 @@ * CVS: $Id$ *************************************************************************/ -#ifdef HAVE_CONFIG_H - #include "config.h" -#endif +#include "config.h" #include <playerconfig.h> Modified: code/player/trunk/server/drivers/localization/amcl/pf/pf_kdtree.c =================================================================== --- code/player/trunk/server/drivers/localization/amcl/pf/pf_kdtree.c 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/localization/amcl/pf/pf_kdtree.c 2008-10-02 00:44:06 UTC (rev 7057) @@ -11,9 +11,7 @@ #include <stdlib.h> #include <string.h> -#ifdef HAVE_CONFIG_H #include "config.h" -#endif #include "pf_vector.h" #include "pf_kdtree.h" Modified: code/player/trunk/server/drivers/mixed/botrics/obot.cc =================================================================== --- code/player/trunk/server/drivers/mixed/botrics/obot.cc 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/mixed/botrics/obot.cc 2008-10-02 00:44:06 UTC (rev 7057) @@ -117,9 +117,7 @@ */ /** @} */ -#ifdef HAVE_CONFIG_H - #include "config.h" -#endif +#include "config.h" #include <errno.h> #include <fcntl.h> Modified: code/player/trunk/server/drivers/mixed/erratic/erratic.cc =================================================================== --- code/player/trunk/server/drivers/mixed/erratic/erratic.cc 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/mixed/erratic/erratic.cc 2008-10-02 00:44:06 UTC (rev 7057) @@ -181,9 +181,7 @@ #include "erratic.h" -#ifdef HAVE_CONFIG_H #include "config.h" -#endif bool debug_mode = FALSE; Modified: code/player/trunk/server/drivers/mixed/irobot/create/create_comms.c =================================================================== --- code/player/trunk/server/drivers/mixed/irobot/create/create_comms.c 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/mixed/irobot/create/create_comms.c 2008-10-02 00:44:06 UTC (rev 7057) @@ -32,9 +32,7 @@ #include <unistd.h> #include <netinet/in.h> -#ifdef HAVE_CONFIG_H - #include "config.h" -#endif +#include "config.h" #include <libplayercore/playercommon.h> #include <playerconfig.h> Modified: code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_comms.c =================================================================== --- code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_comms.c 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_comms.c 2008-10-02 00:44:06 UTC (rev 7057) @@ -32,9 +32,7 @@ #include <unistd.h> #include <netinet/in.h> -#ifdef HAVE_CONFIG_H - #include "config.h" -#endif +#include "config.h" #include <playerconfig.h> #include <replace/replace.h> Modified: code/player/trunk/server/drivers/mixed/mricp/src/mricp_driver.cpp =================================================================== --- code/player/trunk/server/drivers/mixed/mricp/src/mricp_driver.cpp 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/mixed/mricp/src/mricp_driver.cpp 2008-10-02 00:44:06 UTC (rev 7057) @@ -28,9 +28,7 @@ * might be added later on. * By Tarek Taha and Jonathan Paxman / Centre of Autonomous Systems University of Technology Sydney */ -#ifdef HAVE_CONFIG_H - #include "config.h" -#endif +#include <config.h> #include <sys/types.h> #include <sys/stat.h> #include <sys/time.h> Modified: code/player/trunk/server/drivers/mixed/p2os/p2os.cc =================================================================== --- code/player/trunk/server/drivers/mixed/p2os/p2os.cc 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/mixed/p2os/p2os.cc 2008-10-02 00:44:06 UTC (rev 7057) @@ -305,9 +305,7 @@ */ /** @} */ -#ifdef HAVE_CONFIG_H - #include "config.h" -#endif +#include "config.h" #include "p2os.h" #include <libplayerxdr/playerxdr.h> Modified: code/player/trunk/server/drivers/mixed/wbr/914/wbr914.cc =================================================================== --- code/player/trunk/server/drivers/mixed/wbr/914/wbr914.cc 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/mixed/wbr/914/wbr914.cc 2008-10-02 00:44:06 UTC (rev 7057) @@ -101,9 +101,7 @@ */ /** @} */ -#ifdef HAVE_CONFIG_H - #include "config.h" -#endif +#include "config.h" #include <unistd.h> #include <inttypes.h> @@ -297,12 +295,12 @@ _ir_geom.poses[ 7 ].px = 0.200; _ir_geom.poses[ 7 ].py = 0.060; _ir_geom.poses[ 7 ].pyaw = DTOR( 60 ); - + _data.ir.ranges_count = NUM_IR_SENSORS; _data.ir.voltages_count = _data.ir.ranges_count; _data.ir.ranges = new float [_data.ir.ranges_count]; _data.ir.voltages = new float [_data.ir.voltages_count]; - + } /** @@ -316,7 +314,7 @@ delete [] _ir_geom.poses; delete [] _data.ir.ranges; delete [] _data.ir.voltages; - + } int wbr914::Setup() @@ -445,7 +443,7 @@ { printf( "Error setting sign-magnitude mode\n" ); } - + /* This might be a good time to reset the odometry values */ if ( _debug ) printf( "ResetRawPositions\n" ); @@ -883,7 +881,7 @@ { GetPositionData( &_data.position ); } - + if ( ir_subscriptions ) { GetIRData( &_data.ir ); @@ -933,8 +931,8 @@ _yaw -= TWOPI; // calc current x and y position - _x += ( transchange * cos( _yaw )); - _y += ( transchange * sin( _yaw )); + _x += ( transchange * cos( _yaw )); + _y += ( transchange * sin( _yaw )); // add code to read in the speed data @@ -972,7 +970,7 @@ // float v80 = 0.25; // float deltaV = 2.25; // float v10 = v80+deltaV; - // float mmPerVolt = (800.0-100.0)/(v80-v10); + // float mmPerVolt = (800.0-100.0)/(v80-v10); for (uint32_t i=0; i < d->ranges_count; i++) { @@ -1331,7 +1329,7 @@ return -1; } } - + printf( "Write timeout; wrote %ld bytes, tried to write %ld\n", numwritten, len ); @@ -1360,8 +1358,8 @@ return numwritten; } -int wbr914::sendCmdCom( unsigned char address, unsigned char c, - int cmd_num, unsigned char* arg, +int wbr914::sendCmdCom( unsigned char address, unsigned char c, + int cmd_num, unsigned char* arg, int ret_num, unsigned char * ret ) { assert( cmd_num<=4 ); @@ -1369,7 +1367,7 @@ unsigned char cmd[8]; //bool retry = true; unsigned char savecs; - + cmd[0] = address; cmd[1] = 0; // checksum. to be overwritten cmd[2] = 0x00; @@ -1413,7 +1411,7 @@ // if ( _fd_blocking == false ) usleep( DELAY_US ); - int rc; + int rc; if( (rc = ReadBuf( ret, ret_num )) < 0 ) { // printf( "failed to read response\n" ); @@ -1426,13 +1424,13 @@ return result; } -int wbr914::sendCmd0( unsigned char address, unsigned char c, +int wbr914::sendCmd0( unsigned char address, unsigned char c, int ret_num, unsigned char * ret ) { return sendCmdCom( address, c, 0, NULL, ret_num, ret ); } -int wbr914::sendCmd16( unsigned char address, unsigned char c, +int wbr914::sendCmd16( unsigned char address, unsigned char c, int16_t arg, int ret_num, unsigned char * ret ) { @@ -1445,7 +1443,7 @@ return sendCmdCom( address, c, 2, args, ret_num, ret ); } -int wbr914::sendCmd32( unsigned char address, unsigned char c, +int wbr914::sendCmd32( unsigned char address, unsigned char c, int32_t arg, int ret_num, unsigned char * ret ) { unsigned char args[4]; @@ -1690,15 +1688,15 @@ } void wbr914::Stop( int StopMode ) { - + unsigned char ret[8]; - + if ( _debug ) printf( "Stop\n" ); /* Start with motor 0*/ _stopped = true; - + if( StopMode == FULL_STOP ) { if (sendCmd16( LEFT_MOTOR, RESETEVENTSTATUS, 0x0000, 2, ret )<0 ) Modified: code/player/trunk/server/drivers/position/microstrain/3dmg.cc =================================================================== --- code/player/trunk/server/drivers/position/microstrain/3dmg.cc 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/position/microstrain/3dmg.cc 2008-10-02 00:44:06 UTC (rev 7057) @@ -72,9 +72,7 @@ */ /** @} */ -#ifdef HAVE_CONFIG_H - #include "config.h" -#endif +#include "config.h" #include <assert.h> #include <errno.h> Modified: code/player/trunk/server/drivers/position/nav200/sicknav200.cc =================================================================== --- code/player/trunk/server/drivers/position/nav200/sicknav200.cc 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/position/nav200/sicknav200.cc 2008-10-02 00:44:06 UTC (rev 7057) @@ -93,9 +93,7 @@ */ /** @} */ -#if HAVE_CONFIG_H #include <config.h> -#endif #include <assert.h> #include <math.h> @@ -143,10 +141,10 @@ int Initialise(); bool Initialised; - + // set device to positioning mode and set positioning mode parameters int EnterPositioning(); - + // Get device to map reflectors. void UpdateMap(); // Get the reflector positions from the device. @@ -202,10 +200,10 @@ // number of values for slifing mean IntProperty SmoothingInput; - + // radius of the reflectors in meters DoubleProperty ReflectorRadius; - + // storage for outgoing data player_position2d_data_t data_packet; @@ -225,7 +223,7 @@ // Odometry Device info Device *odometry; player_devaddr_t odometry_addr; - + // Opaque Device info Device *opaque; player_devaddr_t opaque_id; @@ -267,7 +265,7 @@ //////////////////////////////////////////////////////////////////////////////// // Constructor SickNAV200::SickNAV200(ConfigFile* cf, int section) : - Driver(cf, section, false), + Driver(cf, section, false), Initialised(false), mode("mode", DEFAULT_SICKNAV200_MODE, false, this, cf, section), Nearest("nearest", 0, false, this, cf, section), @@ -352,16 +350,16 @@ memset(&this->odometry_addr, 0, sizeof(this->odometry_addr)); cf->ReadDeviceAddr(&this->odometry_addr, section, "requires", PLAYER_POSITION2D_CODE, -1, "odometry"); - - + + PLAYER_MSG0(2, "reading reflector map id now"); this->reflector_map = NULL; memset(&this->reflector_map_id, 0, sizeof(this->reflector_map_id)); cf->ReadDeviceAddr(&this->reflector_map_id, section, "requires", PLAYER_VECTORMAP_CODE, -1, NULL); - - + + return; } @@ -405,7 +403,7 @@ return (-1); } } - + if (this->reflector_map_id.interf == PLAYER_VECTORMAP_CODE) // Reflector positions are provided. { if (!(this->reflector_map @@ -485,8 +483,8 @@ odo_pos = recv->pos; return 0; } - - + + if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_GET_GEOM, this->position_addr)) { player_position2d_geom_t geom= { { 0 } }; @@ -727,7 +725,7 @@ } } } - + LaserPos Reading; for (;;) { // test if we are supposed to cancel @@ -762,9 +760,9 @@ usleep(1000); continue; } - + player_pose2d_t nav_vel_WCF = {0,0,0}; - if (velocity) + if (velocity) { nav_vel_WCF = GetNavVelocityInWRF(); short pa_in_bdeg = static_cast<short> (nav_vel_WCF.pa * 32768.0 / M_PI); @@ -807,7 +805,7 @@ // Log the following data: // TIMESTAMP, UPDATE_PERIOD, REQUEST_DELTA, POSE, NAV_POSE, QUALITY,NUM_REFLECTORS, VELOCITY_DELTA, VELOCITY, VELOCITY_NAV_WRF fprintf(TimingLogFile, "%f %f %f %f %f %f %d %d %d %d %d %f %f %f %f %f %f %f\n", - time_double(now), + time_double(now), -static_cast<double> (usec_diff(now, last_nav_position_time))/1e6, static_cast<double> (usec_diff(last_nav_position_time, last_nav_request_time))/1e6, data_packet.pos.px,data_packet.pos.py,data_packet.pos.pa, @@ -819,8 +817,8 @@ fflush(TimingLogFile); } last_nav_position_time = now; - - + + //printf("Got reading: quality %d\n", Reading.quality); if (Reading.quality==0xFF || Reading.quality==0xFE || Reading.quality<=QualityThreshold) { @@ -849,7 +847,7 @@ PLAYER_MSG0(4,"NAV200 using odometry to try recover position\n"); Laser.ChangeLayerDefPosition(0,static_cast<int> (odo_pos.px*1000),static_cast<int> (odo_pos.py*1000),pa_in_bdeg); } - + } else { PLAYER_WARN("Failed to get reading from laser scanner\n"); Initialised = false; Modified: code/player/trunk/server/drivers/ptz/amtecpowercube.cc =================================================================== --- code/player/trunk/server/drivers/ptz/amtecpowercube.cc 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/ptz/amtecpowercube.cc 2008-10-02 00:44:06 UTC (rev 7057) @@ -87,9 +87,7 @@ /** @} */ -#ifdef HAVE_CONFIG_H - #include "config.h" -#endif +#include "config.h" #include <errno.h> #include <fcntl.h> Modified: code/player/trunk/server/drivers/ptz/canonvcc4.cc =================================================================== --- code/player/trunk/server/drivers/ptz/canonvcc4.cc 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/ptz/canonvcc4.cc 2008-10-02 00:44:06 UTC (rev 7057) @@ -72,9 +72,7 @@ */ /** @} */ -#ifdef HAVE_CONFIG_H - #include "config.h" -#endif +#include "config.h" #include <fcntl.h> #include <stdio.h> Modified: code/player/trunk/server/drivers/ptz/sonyevid30.cc =================================================================== --- code/player/trunk/server/drivers/ptz/sonyevid30.cc 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/drivers/ptz/sonyevid30.cc 2008-10-02 00:44:06 UTC (rev 7057) @@ -105,9 +105,7 @@ */ /** @} */ -#ifdef HAVE_CONFIG_H - #include "config.h" -#endif +#include "config.h" #include <fcntl.h> #include <stdio.h> Deleted: code/player/trunk/server/libplayerdrivers/driverregistry.cc =================================================================== --- code/player/trunk/server/libplayerdrivers/driverregistry.cc 2008-10-01 07:41:02 UTC (rev 7056) +++ code/player/trunk/server/libplayerdrivers/driverregistry.cc 2008-10-02 00:44:06 UTC (rev 7057) @@ -1,982 +0,0 @@ -/* - * 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 - * 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 - * - */ -/******************************************************************** - * - * 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 - * - ********************************************************************/ - -/* - * $Id$ - */ -#if HAVE_CONFIG_H - #include <config.h> // to get the INCLUDE_foo macros -#endif - -#include <libplayercore/playercore.h> - -/* prototype driver-specific init funcs */ -#ifdef INCLUDE_ALSA -void Alsa_Register(DriverTable *table); -#endif - -#ifdef INCLUDE_ARTOOLKITPLUS -void ARToolkitPlusDriver_Register(DriverTable *table); -#endif - -#ifdef INCLUDE_BUMPERSAFE -void BumperSafe_Register(DriverTable *table); -#endif - -#ifdef INCLUDE_CHATTERBOX -void Chatterbox_Register(DriverTable *table); -#endif - -#ifdef INCLUDE_MRICP -void MrIcp_Register(DriverTable *table); -#endif - -#ifdef INCLUDE_ND -void ND_Register(DriverTable *table); -#endif - -#ifdef INCLUDE_MBICP -void mbicp_Register(DriverTable *table); -#endif - -#ifdef INCLUDE_GARMINNMEA -void GarminNMEA_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_VMAPFILE -void VMapFile_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_MAPFILE -void MapFile_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_NIMU -void NIMU_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_RELAY -void Relay_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_MAPCSPACE -void MapCspace_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_MAPSCALE -void MapScale_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_AMTECPOWERCUBE -void AmtecPowerCube_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_CLODBUSTER -void ClodBuster_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_OBOT -void Obot_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_ER1 -void ER_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_WAVEFRONT -void Wavefront_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_SEGWAYRMP -void SegwayRMP_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_LASERPOSEINTERPOLATOR -void LaserPoseInterp_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_SICKLMS200 -void SickLMS200_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_SICKLMS400 -void SickLMS400_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_SICKNAV200 -void SickNAV200_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_SICKPLS -// PLS driver was integrated into LMS200 -void SickLMS200_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_RS4LEUZE -void RS4LeuzeLaserDriver_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_PHIDGETRFID -void PhidgetRFID_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_PHIDGETIFK -void PhidgetIFK_Register(DriverTable* table); -#endif - - -#ifdef INCLUDE_SICKRFI341 -void SickRFI341_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_SICKS3000 -void SickS3000_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_SERIALSTREAM -void SerialStream_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_TCPSTREAM -void TCPStream_Register(DriverTable* table); -#endif - - -#ifdef INCLUDE_WBR914 -void wbr914_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_ROOMBA -void Roomba_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_CREATE -void Create_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_GARCIA -void GarciaDriver_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_URGLASER -void URGLaserDriver_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_ACTS -void Acts_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_CMVISION -void CMVision_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_STATGRAB -void StatGrabDriver_Register(DriverTable* table); -#endif - - -#ifdef INCLUDE_CMUCAM2 -void Cmucam2_Register(DriverTable* table); -// REMOVE void Cmucam2blobfinder_Register(DriverTable* table); -// REMOVE void Cmucam2ptz_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_UPCBARCODE -void UPCBarcode_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_SIMPLESHAPE -void SimpleShape_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_FESTIVAL -void Festival_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_SPHINX2 -void Sphinx2_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_LASERBAR -void LaserBar_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_LASERBARCODE -void LaserBarcode_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_LASERVISUALBARCODE -void LaserVisualBarcode_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_LASERVISUALBW -void LaserVisualBW_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_LASERCSPACE -void LaserCSpace_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_LASERRESCAN -void LaserRescan_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_LASERCUTTER -void LaserCutter_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_LASERSAFE -void LaserSafe_Register(DriverTable *table); -#endif - -#ifdef INCLUDE_SONYEVID30 -void SonyEVID30_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_PTU46 -void PTU46_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_CANONVCC4 -void canonvcc4_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_FLOCKOFBIRDS -void FlockOfBirds_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_DUMMY -void Dummy_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_PASSTHROUGH -void PassThrough_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_LOGFILE -void WriteLog_Register(DriverTable* table); -void ReadLog_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_KARTOWRITER -void KartoLogger_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_P2OS -void P2OS_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_ERRATIC -void Erratic_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_RFLEX -void RFLEX_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_LINUXWIFI -void LinuxWiFi_Register(DriverTable *table); -#endif - -#ifdef INCLUDE_AODV -void Aodv_Register(DriverTable *table); -#endif - -#ifdef INCLUDE_IWSPY -void Iwspy_Register(DriverTable *table); -#endif - -#ifdef INCLUDE_LINUXJOYSTICK -void LinuxJoystick_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_REB -void REB_Register(DriverTable *table); -#endif - -#ifdef INCLUDE_KHEPERA -void Khepera_Register(DriverTable *table); -#endif - -#ifdef INCLUDE_MIXER -void Mixer_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_RWI -void RWIBumper_Register(DriverTable* table); -void RWILaser_Register(DriverTable* table); -void RWIPosition_Register(DriverTable* table); -void RWIPower_Register(DriverTable* table); -void RWISonar_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_ISENSE -void InertiaCube2_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_MICROSTRAIN -void MicroStrain3DMG_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_YARPIMAGE -void Yarp_Image_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_MICA2 -void Mica2_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_AMTECM5 -void AmtecM5_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_RCORE_XBRIDGE -void RCore_XBridge_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_SR3000 -void SR3000_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_ACCEL_CALIB -void Accel_Calib_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_XSENSMT -void XSensMT_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_LASERPTZCLOUD -void LaserPTZCloud_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_INAV -void INav_Register(DriverTable *table); -#endif - -#ifdef INCLUDE_VFH -void VFH_Register(DriverTable *table); -#endif - -#ifdef INCLUDE_MCL -void RegularMCL_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_AMCL -void AdaptiveMCL_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_LIFOMCOM -void LifoMCom_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_CAMERAV4L -void CameraV4L_Register(DriverTable *table); -#endif - -#ifdef INCLUDE_CAMERAUVC -void CameraUVC_Register(DriverTable *table); -#endif - -#ifdef INCLUDE_SPHERE -void SphereDriver_Register(DriverTable *table); -#endif - -#ifdef INCLUDE_CAMERA1394 -void Camera1394_Register(DriverTable *table); -#endif - -#ifdef INCLUDE_IMAGESEQ -void ImageSeq_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_CAMERACOMPRESS -void CameraCompress_Register(DriverTable* table); -void CameraUncompress_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_SERVICE_ADV_MDNS -void ServiceAdvMDNS_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_FAKELOCALIZE -void FakeLocalize_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_STAGECLIENT -void StgSimulation_Register(DriverTable *table); -void StgLaser_Register(DriverTable *table); -void StgPosition_Register(DriverTable *table); -void StgSonar_Register(DriverTable *table); -void StgEnergy_Register(DriverTable *table); -void StgBlobfinder_Register(DriverTable *table); -void StgFiducial_Register(DriverTable *table); -//void StgBlinkenlight_Register(DriverTable *table); -#endif - -#ifdef INCLUDE_NOMAD -void Nomad_Register(DriverTable *driverTable); -void NomadPosition_Register(DriverTable *driverTable); -void NomadSonar_Register(DriverTable *driverTable); -//void NomadBumper_Register(DriverTable *driverTable); -//void NomadSpeech_Register(DriverTable *driverTable); -#endif - -#ifdef INCLUDE_INSIDEM300 -void InsideM300_Register(DriverTable *driverTable); -#endif - -#ifdef INCLUDE_SKYETEKM1 -void SkyetekM1_Register(DriverTable *driverTable); -#endif - -#ifdef INCLUDE_LASERTORANGER -void LaserToRanger_Register (DriverTable* table); -#endif - -#ifdef INCLUDE_SONARTORANGER -void SonarToRanger_Register (DriverTable* table); -#endif - -#ifdef INCLUDE_ROBOTEQ -void roboteq_Register (DriverTable* table); -#endif - -#ifdef INCLUDE_ROBOTINO -void robotino_Register (DriverTable* table); -#endif - -#ifdef INCLUDE_PBSLASER -void PBSDriver_Register (DriverTable* table); -#endif - -#ifdef INCLUDE_POSTGIS -void PostGIS_Register (DriverTable* table); -#endif - -#ifdef INCLUDE_VEC2MAP -void Vec2Map_Register(DriverTable * table); -#endif - -#ifdef INCLUDE_BUMPER2LASER -void Bumper2Laser_Register(DriverTable * table); -#endif - -#ifdef INCLUDE_LOCALBB -void LocalBB_Register (DriverTable* table); -#endif - -#ifdef INCLUDE_GBXSICKACFR -void GbxSickAcfr_Register(DriverTable* table); -#endif - -#ifdef INCLUDE_URG_NZ -void UrgDriver_Register(DriverTable* table); -#endif - -/* - * this function will be called at startup. all available devices should - * be added to the driverTable here. they will be instantiated later as - * necessary. - */ -void -player_register_drivers() -{ -#ifdef INCLUDE_ALSA - Alsa_Register(driverTable); -#endif - -#ifdef INCLUDE_ARTOOLKITPLUS - ARToolkitPlusDriver_Register(driverTable); -#endif - -#ifdef INCLUDE_BUMPERSAFE - BumperSafe_Register(driverTable); -#endif - -#ifdef INCLUDE_MRICP - MrIcp_Register(driverTable); -#endif - - #ifdef INCLUDE_ND - ND_Register(driverTable); -#endif - -#ifdef INCLUDE_MBICP - mbicp_Register(driverTable); -#endif - -#ifdef INCLUDE_GARMINNMEA - GarminNMEA_Register(driverTable); -#endif - -#ifdef INCLUDE_MAPFILE - MapFile_Register(driverTable); -#endif - -#ifdef INCLUDE_VMAPFILE - VMapFile_Register(driverTable); -#endif - -#ifdef INCLUDE_NIMU - NIMU_Register(driverTable); -#endif - -#ifdef INCLUDE_RELAY - Relay_Register(driverTable); -#endif - -#ifdef INCLUDE_MAPCSPACE - MapCspace_Register(driverTable); -#endif - -#ifdef INCLUDE_MAPSCALE - MapScale_Register(driverTable); -#endif - -#ifdef INCLUDE_AMTECPOWERCUBE - AmtecPowerCube_Register(driverTable); -#endif - -#ifdef INCLUDE_CLODBUSTER - ClodBuster_Register(driverTable); -#endif - -#ifdef INCLUDE_OBOT - Obot_Register(driverTable); -#endif - -#ifdef INCLUDE_ER1 - ER_Register(driverTable); -#endif - -#ifdef INCLUDE_WAVEFRONT - Wavefront_Register(driverTable); -#endif - -#ifdef INCLUDE_SEGWAYRMP - SegwayRMP_Register(driverTable); -#endif - -#ifdef INCLUDE_LASERPOSEINTERPOLATOR - LaserPoseInterp_Register(driverTable); -#endif - -#ifdef INCLUDE_GARCIA - GarciaDriver_Register(driverTable); -#endif - -#ifdef INCLUDE_SICKLMS200 - SickLMS200_Register(driverTable); -#endif - -#ifdef INCLUDE_SICKLMS400 - SickLMS400_Register(driverTable); -#endif - -#ifdef INCLUDE_SICKNAV200 - SickNAV200_Register(driverTable); -#endif - -#ifdef INCLUDE_RS4LEUZE - RS4LeuzeLaserDriver_Register(driverTable); -#endif - -#ifdef INCLUDE_SICKPLS -#ifndef INCLUDE_SICKLMS200 - SickLMS200_Register(driverTable)... [truncated message content] |
From: <gb...@us...> - 2008-10-07 00:50:10
|
Revision: 7068 http://playerstage.svn.sourceforge.net/playerstage/?rev=7068&view=rev Author: gbiggs Date: 2008-10-07 00:49:03 +0000 (Tue, 07 Oct 2008) Log Message: ----------- Applied and cleaned up most of patch 2145081. Modified Paths: -------------- code/player/trunk/client_libs/libplayerc/client.c code/player/trunk/libplayertcp/playerudp.cc code/player/trunk/libplayertcp/remote_driver.cc code/player/trunk/libplayertcp/socket_util.c code/player/trunk/server/drivers/blobfinder/acts/acts.cc code/player/trunk/server/drivers/laser/lms400_cola.cc code/player/trunk/server/drivers/laser/lms400_cola.h code/player/trunk/server/drivers/mixed/p2os/p2os.cc code/player/trunk/server/drivers/speech/festival.cc code/player/trunk/utils/playerv/playerv.c Modified: code/player/trunk/client_libs/libplayerc/client.c =================================================================== --- code/player/trunk/client_libs/libplayerc/client.c 2008-10-06 20:33:32 UTC (rev 7067) +++ code/player/trunk/client_libs/libplayerc/client.c 2008-10-07 00:49:03 UTC (rev 7068) @@ -46,6 +46,7 @@ #include <assert.h> #include <math.h> +#include <stddef.h> #include <stdio.h> #include <stdlib.h> #include <string.h> @@ -211,7 +212,11 @@ // Connect to the server int playerc_client_connect(playerc_client_t *client) { - struct hostent* entp; +#if defined(HAVE_GETADDRINFO) + struct addrinfo* addr_ptr = NULL; +#else + struct hostent* entp = NULL; +#endif char banner[PLAYER_IDENT_STRLEN]; int old_flags; int ret; @@ -239,7 +244,7 @@ * * Specifying sin_port = 0 allows the system to choose the port. */ - clientaddr.sin_family = PF_INET; + clientaddr.sin_family = AF_INET; clientaddr.sin_addr.s_addr = INADDR_ANY; clientaddr.sin_port = 0; @@ -247,7 +252,7 @@ (struct sockaddr*)&clientaddr, sizeof(clientaddr)) < -1) { PLAYERC_ERR1("bind call failed with error [%s]", strerror(errno)); - return(-1); + return -1; } } else @@ -273,17 +278,38 @@ #endif // Construct server address + memset(&client->server, 0, sizeof(client->server)); + client->server.sin_family = AF_INET; + client->server.sin_port = htons(client->port); +#if defined(HAVE_GETADDRINFO) + if (getaddrinfo(client->host, NULL, NULL, &addr_ptr) != 0) + { + playerc_client_disconnect(client); + PLAYERC_ERR("getaddrinfo() failed with error"); + return -1; + } + assert(addr_ptr); + assert(addr_ptr->ai_addr); + if ((addr_ptr->ai_addr->sa_family) != AF_INET) + { + playerc_client_disconnect(client); + PLAYERC_ERR("unsupported internet address family"); + return -1; + } + client->server.sin_addr.s_addr = ((struct sockaddr_in *)(addr_ptr->ai_addr))->sin_addr.s_addr; + freeaddrinfo(addr_ptr); + addr_ptr = NULL; +#else entp = gethostbyname(client->host); if (entp == NULL) { playerc_client_disconnect(client); - PLAYERC_ERR1("gethostbyname failed with error [%s]", strerror(errno)); + PLAYERC_ERR1("gethostbyname() failed with error [%s]", strerror(errno)); return -1; } - client->server.sin_family = PF_INET; - memcpy(&client->server.sin_addr, entp->h_addr_list[0], entp->h_length); - client->server.sin_port = htons(client->port); - + assert(entp->h_length <= sizeof (client->server.sin_addr)); + memcpy(&(client->server.sin_addr), entp->h_addr_list[0], entp->h_length); +#endif // Connect the socket /* t = client->request_timeout; @@ -363,7 +389,7 @@ { if(send(client->sock, NULL, 0, 0) < 0) { - PLAYERC_ERR1("gethostbyname failed with error [%s]", strerror(errno)); + PLAYERC_ERR1("send() failed with error [%s]", strerror(errno)); return -1; } } Modified: code/player/trunk/libplayertcp/playerudp.cc =================================================================== --- code/player/trunk/libplayertcp/playerudp.cc 2008-10-06 20:33:32 UTC (rev 7067) +++ code/player/trunk/libplayertcp/playerudp.cc 2008-10-07 00:49:03 UTC (rev 7068) @@ -1262,7 +1262,7 @@ * * Specifying sin_port = 0 would allow the system to choose the port. */ - serverp.sin_family = PF_INET; + serverp.sin_family = AF_INET; serverp.sin_addr.s_addr = INADDR_ANY; if(bind(sock, (struct sockaddr*)&serverp, sizeof(serverp)) == -1) Modified: code/player/trunk/libplayertcp/remote_driver.cc =================================================================== --- code/player/trunk/libplayertcp/remote_driver.cc 2008-10-06 20:33:32 UTC (rev 7067) +++ code/player/trunk/libplayertcp/remote_driver.cc 2008-10-07 00:49:03 UTC (rev 7068) @@ -95,7 +95,7 @@ return(-1); } - server.sin_family = PF_INET; + server.sin_family = AF_INET; server.sin_addr.s_addr = this->device_addr.host; server.sin_port = htons(this->device_addr.robot); Modified: code/player/trunk/libplayertcp/socket_util.c =================================================================== --- code/player/trunk/libplayertcp/socket_util.c 2008-10-06 20:33:32 UTC (rev 7067) +++ code/player/trunk/libplayertcp/socket_util.c 2008-10-07 00:49:03 UTC (rev 7068) @@ -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 @@ -59,10 +59,10 @@ #include "socket_util.h" /* - * this function creates a socket of the indicated type and binds it to + * this function creates a socket of the indicated type and binds it to * the indicated port. * - * NOTE: we pick the IP address (and thus network interface) for binding + * NOTE: we pick the IP address (and thus network interface) for binding * by calling gethostname() and then stripping it down to the first * component (i.e. no domain name). if this process won't * result in the IP address that you want, tough luck. @@ -75,14 +75,14 @@ * PLAYER_TRANSPORT_TCP (for TCP) * backlog: number of waiting connections to be allowed (TCP only) * - * RETURN: - * On success, the fd of the new socket is returned. Otherwise, -1 + * RETURN: + * On success, the fd of the new socket is returned. Otherwise, -1 * is returned and an explanatory note is dumped to stderr. */ int -create_and_bind_socket(char blocking, unsigned int host, int* portnum, +create_and_bind_socket(char blocking, unsigned int host, int* portnum, int playersocktype, int backlog) { int sock; /* socket we're creating */ @@ -108,11 +108,11 @@ serverp.sin_addr.s_addr = host; serverp.sin_port = htons(*portnum); - /* - * Create the INET socket. - * + /* + * Create the INET socket. + * */ - if((sock = socket(PF_INET, socktype, 0)) == -1) + if((sock = socket(PF_INET, socktype, 0)) == -1) { perror("create_and_bind_socket:socket() failed; socket not created."); return(-1); @@ -135,14 +135,14 @@ * get the current access flags */ if((flags = fcntl(sock, F_GETFL)) == -1) - { + { perror("create_and_bind_socket():fcntl() while getting socket " "access flags; socket not created."); close(sock); return(-1); } /* - * OR the current flags with O_NONBLOCK (so we won't block), + * OR the current flags with O_NONBLOCK (so we won't block), * and write them back */ if(fcntl(sock, F_SETFL, flags | O_NONBLOCK ) == -1) @@ -157,7 +157,7 @@ if(socktype == SOCK_STREAM) { /* make sure we can reuse the port soon after */ - if(setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, (const char*)&one, + if(setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, (const char*)&one, sizeof(one))) { perror("create_and_bind_socket(): setsockopt(2) failed"); @@ -166,18 +166,18 @@ } - /* + /* * Bind it to the port indicated * INADDR_ANY indicates that any network interface (IP address) - * for the local host may be used (presumably the OS will choose the + * for the local host may be used (presumably the OS will choose the * right one). * * Specifying sin_port = 0 would allow the system to choose the port. */ - serverp.sin_family = PF_INET; + serverp.sin_family = AF_INET; serverp.sin_addr.s_addr = INADDR_ANY; - if(bind(sock, (struct sockaddr*)&serverp, sizeof(serverp)) == -1) + if(bind(sock, (struct sockaddr*)&serverp, sizeof(serverp)) == -1) { perror ("create_and_bind_socket():bind() failed; socket not created."); close(sock); @@ -209,7 +209,7 @@ /* - * return the fd for the newly bound socket + * return the fd for the newly bound socket */ return(sock); } Modified: code/player/trunk/server/drivers/blobfinder/acts/acts.cc =================================================================== --- code/player/trunk/server/drivers/blobfinder/acts/acts.cc 2008-10-06 20:33:32 UTC (rev 7067) +++ code/player/trunk/server/drivers/blobfinder/acts/acts.cc 2008-10-07 00:49:03 UTC (rev 7068) @@ -139,7 +139,10 @@ /** @} */ +#include "config.h" + #include <assert.h> +#include <stddef.h> #include <stdio.h> #include <unistd.h> /* close(2),fcntl(2),getpid(2),usleep(3),execvp(3),fork(2)*/ #include <netdb.h> /* for gethostbyname(3) */ @@ -479,7 +482,6 @@ static struct sockaddr_in server; char host[] = "localhost"; - struct hostent* entp; printf("ACTS vision server connection initializing..."); fflush(stdout); @@ -671,13 +673,33 @@ } else { - /* in parent */ - /* fill in addr structure */ - server.sin_family = PF_INET; + memset(&server, 0, sizeof server); + server.sin_port = htons(portnum); + server.sin_family = AF_INET; +#if HAVE_GETADDRINFO + struct addrinfo* addr_ptr = NULL; + if (getaddrinfo(host, NULL, NULL, &addr_ptr)) + { + PLAYER_ERROR("getaddrinfo() failed with error"); + KillACTS(); + return(1); + } + assert(addr_ptr); + assert(addr_ptr->ai_addr); + if ((addr_ptr->ai_addr->sa_family) != AF_INET) + { + PLAYER_ERROR("unsupported internet address family"); + KillACTS(); + return(1); + } + server.sin_addr.s_addr = (reinterpret_cast<struct sockaddr_in *>(addr_ptr->ai_addr))->sin_addr.s_addr; + freeaddrinfo(addr_ptr); addr_ptr = NULL; +#else /* * this is okay to do, because gethostbyname(3) does no lookup if the * 'host' * arg is already an IP addr */ + struct hostent* entp; if((entp = gethostbyname(host)) == NULL) { fprintf(stderr, "Acts::Setup(): \"%s\" is unknown host; " @@ -688,9 +710,7 @@ } memcpy(&server.sin_addr, entp->h_addr_list[0], entp->h_length); - - server.sin_port = htons(portnum); - +#endif /* ok, we'll make this a bit smarter. first, we wait a baseline amount * of time, then try to connect periodically for some predefined number * of times @@ -710,7 +730,7 @@ KillACTS(); return(1); } - if(connect(sock,(struct sockaddr*)&server, sizeof(server)) == 0) + if(connect(sock,reinterpret_cast<struct sockaddr*> (&server), sizeof(server)) == 0) break; usleep(ACTS_STARTUP_INTERVAL_USEC); } Modified: code/player/trunk/server/drivers/laser/lms400_cola.cc =================================================================== --- code/player/trunk/server/drivers/laser/lms400_cola.cc 2008-10-06 20:33:32 UTC (rev 7067) +++ code/player/trunk/server/drivers/laser/lms400_cola.cc 2008-10-07 00:49:03 UTC (rev 7068) @@ -4,13 +4,17 @@ Date: 7 Feb 2007 CVS: $Id$ */ + +#include "config.h" + +#include <stddef.h> #include <sys/socket.h> #include <netdb.h> #include <libplayercore/playercore.h> #include "lms400_cola.h" #include <unistd.h> -#include <strings.h> +#include <string.h> //////////////////////////////////////////////////////////////////////////////// // Constructor. @@ -35,20 +39,35 @@ return (-1); // Get the network host entry - server = gethostbyname ((const char *)hostname); - if (server == NULL) + memset (&serv_addr, 0, sizeof (serv_addr)); + serv_addr.sin_port = htons(portno); + serv_addr.sin_family = AF_INET; +#if defined (HAVE_GETADDRINFO) + addr_ptr = NULL; + if (getaddrinfo (hostname, NULL, NULL, &(addr_ptr))) + { + PLAYER_ERROR ("getaddrinfo() failed with error"); return (-1); + } + assert (addr_ptr); + assert (addr_ptr->ai_addr); + if ((addr_ptr->ai_addr->sa_family) != AF_INET) + { + PLAYER_ERROR ("unsupported internet address family"); + return (-1); + } + serv_addr.sin_addr.s_addr = (reinterpret_cast<struct sockaddr_in*> (addr_ptr->ai_addr))->sin_addr.s_addr; + freeaddrinfo (addr_ptr); + addr_ptr = NULL; +#else + server = gethostbyname (hostname); + if ((server) == NULL) + return (-1); + memcpy (&(serv_addr.sin_addr.s_addr), server->h_addr, server->h_length); +#endif - // Fill in the sockaddr_in structure values - memset ((char *) &serv_addr, 0, sizeof (serv_addr)); - serv_addr.sin_family = AF_INET; - serv_addr.sin_port = htons (portno); - memcpy ((char *)&serv_addr.sin_addr.s_addr, - (char *)server->h_addr, - server->h_length); - // Attempt to connect - if (connect (sockfd, (const sockaddr*)&serv_addr, sizeof (serv_addr)) < 0) + if (connect (sockfd, reinterpret_cast<struct sockaddr*> (&serv_addr), sizeof (serv_addr)) < 0) return (-1); return (0); Modified: code/player/trunk/server/drivers/laser/lms400_cola.h =================================================================== --- code/player/trunk/server/drivers/laser/lms400_cola.h 2008-10-06 20:33:32 UTC (rev 7067) +++ code/player/trunk/server/drivers/laser/lms400_cola.h 2008-10-07 00:49:03 UTC (rev 7068) @@ -4,6 +4,10 @@ Date: 7 Feb 2007 CVS: $Id$ */ + +#include "config.h" + +#include <netdb.h> #include <sys/types.h> #include <vector> #include <netinet/in.h> @@ -86,7 +90,11 @@ const char* hostname; int sockfd, portno, n; struct sockaddr_in serv_addr; +#if defined (HAVE_GETADDRINFO) + struct addrinfo *addr_ptr; +#else struct hostent *server; +#endif // Internal Parameters: int verbose; Modified: code/player/trunk/server/drivers/mixed/p2os/p2os.cc =================================================================== --- code/player/trunk/server/drivers/mixed/p2os/p2os.cc 2008-10-06 20:33:32 UTC (rev 7067) +++ code/player/trunk/server/drivers/mixed/p2os/p2os.cc 2008-10-07 00:49:03 UTC (rev 7068) @@ -717,22 +717,37 @@ return(1); } //printf("created socket %d.\nLooking up hostname...\n", this->psos_fd); + struct sockaddr_in addr; + memset(&addr, 0, sizeof addr); + addr.sin_family = AF_INET; + addr.sin_port = htons(this->psos_tcp_port); +#if HAVE_GETADDRINFO + struct addrinfo * addr_ptr = NULL; + if (getaddrinfo(this->psos_tcp_host, NULL, NULL, &addr_ptr)) + { + PLAYER_ERROR("Error looking up hostname or address"); + return 1; + } + assert(addr_ptr); + assert(addr_ptr->ai_addr); + assert((addr_ptr->ai_addr->sa_family) == AF_INET); + addr.sin_addr.s_addr = (reinterpret_cast<struct sockaddr_in *>(addr_ptr->ai_addr))->sin_addr.s_addr; + freeaddrinfo(addr_ptr); + addr_ptr = NULL; +#else struct hostent* h = gethostbyname(this->psos_tcp_host); if(!h) { perror("Error looking up hostname or address %s:"); return(1); } - struct sockaddr_in addr; - assert((size_t)h->h_length <= sizeof(addr.sin_addr)); + assert(static_cast<size_t> (h->h_length) <= sizeof(addr.sin_addr)); //printf("gethostbyname returned address %d length %d.\n", * h->h_addr, h->h_length); memcpy(&(addr.sin_addr), h->h_addr, h->h_length); //printf("copied address to addr.sin_addr.s_addr=%d\n", addr.sin_addr.s_addr); - addr.sin_family = AF_INET; - addr.sin_port = htons(this->psos_tcp_port); - printf("Found host address, connecting..."); - fflush(stdout); - if(connect(this->psos_fd, (struct sockaddr*) &addr, sizeof(addr)) < 0) +#endif + PLAYER_WARN("Found host address, connecting..."); + if(connect(this->psos_fd, reinterpret_cast<struct sockaddr*> (&addr), sizeof(addr)) < 0) { perror("Error Connecting to remote host (P2OS::Setup()::connect()):"); return(1); @@ -746,7 +761,7 @@ return(1); } assert(flags & O_NONBLOCK); - printf("TCP socket connection is OK... "); + PLAYER_WARN("TCP socket connection is OK... "); fflush(stdout); } else Modified: code/player/trunk/server/drivers/speech/festival.cc =================================================================== --- code/player/trunk/server/drivers/speech/festival.cc 2008-10-06 20:33:32 UTC (rev 7067) +++ code/player/trunk/server/drivers/speech/festival.cc 2008-10-07 00:49:03 UTC (rev 7068) @@ -79,8 +79,12 @@ */ /** @} */ +#include "config.h" + +#include <stddef.h> #include <stdio.h> #include <errno.h> +#include <assert.h> #include <unistd.h> /* close(2),fcntl(2),getpid(2),usleep(3),execlp(3),fork(2)*/ #include <netdb.h> /* for gethostbyname(3) */ #include <netinet/in.h> /* for struct sockaddr_in, htons(3) */ @@ -229,9 +233,8 @@ char* festival_args[8]; - static struct sockaddr_in server; + struct sockaddr_in server; char host[] = "localhost"; - struct hostent* entp; // start out with a clean slate //queue->Flush(); @@ -286,29 +289,44 @@ } else { - /* in parent */ - /* fill in addr structure */ - server.sin_family = PF_INET; + memset(&server, 0, sizeof (server)); + server.sin_family = AF_INET; + server.sin_port = htons(portnum); +#if HAVE_GETADDRINFO + struct addrinfo* addr_ptr = NULL; + if (getaddrinfo(host, NULL, NULL, &addr_ptr)) + { + PLAYER_ERROR("getaddrinfo() failed with error"); + KillFestival(); + return(1); + } + assert(addr_ptr); + assert(addr_ptr->ai_addr); + if ((addr_ptr->ai_addr->sa_family) != AF_INET) + { + PLAYER_ERROR("unsupported internet address family"); + KillFestival(); + return(1); + } + server.sin_addr.s_addr = (reinterpret_cast<struct sockaddr_in *>(addr_ptr->ai_addr))->sin_addr.s_addr; + freeaddrinfo(addr_ptr); + addr_ptr = NULL; +#else /* * this is okay to do, because gethostbyname(3) does no lookup if the * 'host' * arg is already an IP addr */ + struct hostent* entp; if((entp = gethostbyname(host)) == NULL) { - fprintf(stderr, "Festival::Setup(): \"%s\" is unknown host; " + PLAYER_ERROR1("Festival::Setup(): \"%s\" is unknown host; " "can't connect to Festival\n", host); /* try to kill Festival just in case it's running */ KillFestival(); return(1); } +#endif - memcpy(&server.sin_addr, entp->h_addr_list[0], entp->h_length); - - - server.sin_port = htons(portnum); - - - /* ok, we'll make this a bit smarter. first, we wait a baseline amount * of time, then try to connect periodically for some predefined number * of times @@ -328,7 +346,7 @@ /* * hook it up */ - if(connect(sock, (struct sockaddr*)&server, sizeof(server)) == 0) + if(connect(sock, reinterpret_cast<struct sockaddr*> (&server), sizeof(server)) == 0) break; usleep(FESTIVAL_STARTUP_INTERVAL_USEC); } Modified: code/player/trunk/utils/playerv/playerv.c =================================================================== --- code/player/trunk/utils/playerv/playerv.c 2008-10-06 20:33:32 UTC (rev 7067) +++ code/player/trunk/utils/playerv/playerv.c 2008-10-07 00:49:03 UTC (rev 7068) @@ -314,7 +314,7 @@ // start out timer if in pull mode if(rate > 0.0) gettimeofday(&tv, NULL); - + while (!quit) { // Let gui process messages 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:30:35
|
Revision: 7074 http://playerstage.svn.sourceforge.net/playerstage/?rev=7074&view=rev Author: thjc Date: 2008-10-07 10:27:52 +0000 (Tue, 07 Oct 2008) Log Message: ----------- Merge from 2-1 to trunk 6985 P2-1 fixed overflow in timeout calculation Modified Paths: -------------- code/player/trunk/libplayercore/message.cc Property Changed: ---------------- code/player/trunk/ Property changes on: code/player/trunk ___________________________________________________________________ Modified: svn:mergeinfo - /code/player/branches/release-2-1-patches:6672-6673,6738,6834,6886,6905,6930 + /code/player/branches/release-2-1-patches:6672-6673,6738,6834,6886,6905,6930,6936,6946,6951,6955,6975,6979-6981,6985 /code/player/trunk:6985 Modified: code/player/trunk/libplayercore/message.cc =================================================================== --- code/player/trunk/libplayercore/message.cc 2008-10-07 10:19:43 UTC (rev 7073) +++ code/player/trunk/libplayercore/message.cc 2008-10-07 10:27:52 UTC (rev 7074) @@ -59,7 +59,7 @@ Message::Message(const struct player_msghdr & aHeader, void * data, - bool copy) + bool copy) { CreateMessage(aHeader, data, copy); } @@ -96,7 +96,7 @@ void Message::CreateMessage(const struct player_msghdr & aHeader, void * data, - bool copy) + bool copy) { this->Lock = new pthread_mutex_t; assert(this->Lock); @@ -335,8 +335,9 @@ { struct timespec tp; clock_gettime(CLOCK_REALTIME, &tp); + TimeOut += static_cast<double> (tp.tv_nsec) * 1e-9; tp.tv_sec += static_cast<int> (floor(TimeOut)); - tp.tv_nsec += static_cast<int> ((TimeOut - floor(TimeOut))*1e9); + tp.tv_nsec = static_cast<int> ((TimeOut - floor(TimeOut))*1e9); int ret = pthread_cond_timedwait(&this->cond, &this->condMutex, &tp); // if we got an error or timed out if (ret != 0) @@ -344,9 +345,9 @@ } else { - pthread_cond_wait(&this->cond,&this->condMutex); + pthread_cond_wait(&this->cond,&this->condMutex); } - + pthread_mutex_unlock(&this->condMutex); pthread_cleanup_pop(0); return result; @@ -412,15 +413,15 @@ pthread_cond_broadcast(&this->cond); pthread_mutex_unlock(&this->condMutex); } - + /// @brief Set the data_requested flag -void +void MessageQueue::SetDataRequested(bool d, bool haveLock) -{ +{ if(!haveLock) this->Lock(); - this->data_requested = d; - this->data_delivered = false; + this->data_requested = d; + this->data_delivered = false; if(!haveLock) this->Unlock(); } @@ -453,7 +454,7 @@ this->Unlock(); } -/// Push a message at the back of the queue, without checking replacement +/// Push a message at the back of the queue, without checking replacement /// rules or size limits. /// This method is used internally to insert most messages. /// The caller may have already called Lock() on this queue @@ -566,7 +567,7 @@ for(el = this->head; el; el = el->next) { if(resp_el || - ((!this->filter_on || this->Filter(*el->msg)) && + ((!this->filter_on || this->Filter(*el->msg)) && (!this->pull || this->data_requested))) { if(el == resp_el) @@ -583,7 +584,7 @@ } // queue is empty. if that data had been requested in pull mode, and - // some has been delivered, then mark the end of this frame with a + // some has been delivered, then mark the end of this frame with a // sync message if(this->pull && this->data_requested && this->data_delivered) { @@ -650,7 +651,7 @@ this->RefCount = new unsigned int; assert(this->RefCount); - *this->RefCount = 1; + *this->RefCount = 1; } /// Destroy our reference to the message queue. @@ -667,7 +668,7 @@ { Lock = NULL; RefCount = NULL; - Queue = NULL; + Queue = NULL; } else { @@ -681,19 +682,19 @@ Queue = rhs.Queue; RefCount = rhs.RefCount; (*RefCount)++; - pthread_mutex_unlock(Lock); + pthread_mutex_unlock(Lock); } } - + /// assign reference to our message queue QueuePointer & QueuePointer::operator = (const QueuePointer & rhs) { // first remove our current reference DecRef(); - + if (rhs.Queue == NULL) return *this; - + // then copy the rhs assert(rhs.Lock); pthread_mutex_lock(rhs.Lock); @@ -705,11 +706,11 @@ Queue = rhs.Queue; RefCount = rhs.RefCount; (*RefCount)++; - pthread_mutex_unlock(Lock); + pthread_mutex_unlock(Lock); return *this; } -MessageQueue * QueuePointer::get() const +MessageQueue * QueuePointer::get() const { return Queue; } @@ -719,45 +720,45 @@ MessageQueue * QueuePointer::operator -> () { assert(Queue); - return Queue; + return Queue; } /// retrieve underlying object for use MessageQueue & QueuePointer::operator * () { assert(Queue); - return *Queue; + return *Queue; } /// check if pointers are equal bool QueuePointer::operator == (const QueuePointer & rhs) { - return rhs.Queue == Queue; + return rhs.Queue == Queue; } /// check if pointers are equal bool QueuePointer::operator == (void * pointer) { - return Queue == pointer; + return Queue == pointer; } /// check if pointers are equal bool QueuePointer::operator != (const QueuePointer & rhs) { - return rhs.Queue != Queue; + return rhs.Queue != Queue; } /// check if pointers are equal bool QueuePointer::operator != (void * pointer) { - return Queue != pointer; + return Queue != pointer; } void QueuePointer::DecRef() { if (Queue == NULL) return; - + pthread_mutex_lock(Lock); (*RefCount)--; assert((*RefCount) >= 0); @@ -776,7 +777,7 @@ { Queue = NULL; RefCount = NULL; - pthread_mutex_unlock(Lock); + pthread_mutex_unlock(Lock); Lock = NULL; } } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2008-10-07 11:23:29
|
Revision: 7075 http://playerstage.svn.sourceforge.net/playerstage/?rev=7075&view=rev Author: thjc Date: 2008-10-07 10:42:15 +0000 (Tue, 07 Oct 2008) Log Message: ----------- Merging changes from 2-1 to trunk 6987 P2-1 fixed up colour setting in graphics3d example 6989 P2-1 applied [ 2082498 ] fixed Geometry-Request 6990 P2-1 [ 2074625 ] serialstream fix for osx 6991 P2-1 aplpied [ 2046418 ] roomba driver: stall notification 6992 P2-1 applied [ 2016923 ] Error checking and better timeouts in SICK LMS200 6993 P2-1 Applied [ 1998688 ] camerav4l patch 6994 P2-1 Applied [ 1957498 ] Fixes for segfaults on the InsideM300 RFID driver 6996 P2-1 added headers suggested by [ 2020265 ] gcc 4.3 compatibility Modified Paths: -------------- code/player/trunk/client_libs/libplayerc++/playerc++.h code/player/trunk/examples/libplayerc++/clientgraphics3d.cc code/player/trunk/server/drivers/actarray/amtecM5.cc code/player/trunk/server/drivers/camera/v4l/camerav4l.cc code/player/trunk/server/drivers/laser/lms400_cola.h code/player/trunk/server/drivers/laser/sicklms200.cc code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_driver.cc code/player/trunk/server/drivers/mixed/mricp/src/geometry2D.cpp code/player/trunk/server/drivers/opaque/serialstream.cc code/player/trunk/server/drivers/rfid/insideM300.cc Property Changed: ---------------- code/player/trunk/ Property changes on: code/player/trunk ___________________________________________________________________ Modified: svn:mergeinfo - /code/player/branches/release-2-1-patches:6672-6673,6738,6834,6886,6905,6930,6936,6946,6951,6955,6975,6979-6981,6985 /code/player/trunk:6985 + /code/player/branches/release-2-1-patches:6672-6673,6738,6834,6886,6905,6930,6936,6946,6951,6955,6975,6979-6981,6985,6987-6996 /code/player/trunk:6985 Modified: code/player/trunk/client_libs/libplayerc++/playerc++.h =================================================================== --- code/player/trunk/client_libs/libplayerc++/playerc++.h 2008-10-07 10:27:52 UTC (rev 7074) +++ code/player/trunk/client_libs/libplayerc++/playerc++.h 2008-10-07 10:42:15 UTC (rev 7075) @@ -53,6 +53,7 @@ #include <string> #include <list> #include <vector> +#include <cstring> #include "libplayerc/playerc.h" #include "libplayerc++/utility.h" Modified: code/player/trunk/examples/libplayerc++/clientgraphics3d.cc =================================================================== --- code/player/trunk/examples/libplayerc++/clientgraphics3d.cc 2008-10-07 10:27:52 UTC (rev 7074) +++ code/player/trunk/examples/libplayerc++/clientgraphics3d.cc 2008-10-07 10:42:15 UTC (rev 7075) @@ -57,7 +57,8 @@ for( r=3; r>0; r-=0.1 ) { col.blue = (int)(r * 255.0 /3); - col.red = (int)((255.0 - r * 255.0)/3); + col.red = 255 - col.blue; + gp.Color( col ); player_point_3d_t pts[4]; pts[0].px = -r; Modified: code/player/trunk/server/drivers/actarray/amtecM5.cc =================================================================== --- code/player/trunk/server/drivers/actarray/amtecM5.cc 2008-10-07 10:27:52 UTC (rev 7074) +++ code/player/trunk/server/drivers/actarray/amtecM5.cc 2008-10-07 10:42:15 UTC (rev 7075) @@ -600,14 +600,14 @@ pclDevice->getConfig(idModuleList[i],&configword); uint8_t hasbrakes=0; - if (configword && CONFIGID_MOD_BRAKE_PRESENT) { + if (configword & CONFIGID_MOD_BRAKE_PRESENT) { hasbrakes=1; } else { hasbrakes=0; } uint8_t type=0; - if (configword && CONFIGID_MOD_LINEAR) { + if (configword & CONFIGID_MOD_LINEAR) { type=PLAYER_ACTARRAY_TYPE_LINEAR; } else { type=PLAYER_ACTARRAY_TYPE_ROTARY; Modified: code/player/trunk/server/drivers/camera/v4l/camerav4l.cc =================================================================== --- code/player/trunk/server/drivers/camera/v4l/camerav4l.cc 2008-10-07 10:27:52 UTC (rev 7074) +++ code/player/trunk/server/drivers/camera/v4l/camerav4l.cc 2008-10-07 10:42:15 UTC (rev 7075) @@ -174,7 +174,8 @@ #include <errno.h> #include <string.h> #include <math.h> -#include <stdlib.h> // for atoi(3) +#include <stddef.h> +#include <stdlib.h> // for malloc() and free() #include <unistd.h> #include <time.h> @@ -220,6 +221,10 @@ // Pixel depth private: int depth; + // Format + + private: uint32_t format; + // Camera palette private: const char *palette; @@ -263,16 +268,13 @@ private: time_t publish_time; - // Data to send to server - private: player_camera_data_t data; - }; // Initialization function Driver* CameraV4L_Init( ConfigFile* cf, int section) { - return ((Driver*) (new CameraV4L( cf, section))); + return (reinterpret_cast<Driver *>(new CameraV4L( cf, section))); } @@ -373,28 +375,28 @@ { fg_set_format(this->fg, VIDEO_PALETTE_GREY); this->frame = frame_new(this->width, this->height, VIDEO_PALETTE_GREY ); - this->data.format = PLAYER_CAMERA_FORMAT_MONO8; + this->format = PLAYER_CAMERA_FORMAT_MONO8; this->depth = 8; } else if (strcasecmp(this->palette, "RGB565") == 0) { fg_set_format(this->fg, VIDEO_PALETTE_RGB565 ); this->frame = frame_new(this->width, this->height, VIDEO_PALETTE_RGB565 ); - this->data.format = PLAYER_CAMERA_FORMAT_RGB565; + this->format = PLAYER_CAMERA_FORMAT_RGB565; this->depth = 16; } else if (strcasecmp(this->palette, "RGB888") == 0) { fg_set_format(this->fg, VIDEO_PALETTE_RGB24 ); this->frame = frame_new(this->width, this->height, VIDEO_PALETTE_RGB24 ); - this->data.format = PLAYER_CAMERA_FORMAT_RGB888; + this->format = PLAYER_CAMERA_FORMAT_RGB888; this->depth = 24; } else if (strcasecmp(this->palette, "RGB32") == 0) { fg_set_format(this->fg, VIDEO_PALETTE_RGB32 ); this->frame = frame_new(this->width, this->height, VIDEO_PALETTE_RGB32 ); - this->data.format = PLAYER_CAMERA_FORMAT_RGB888; + this->format = PLAYER_CAMERA_FORMAT_RGB888; this->depth = 32; } else if (strcasecmp(this->palette, "YUV420P") == 0) @@ -403,25 +405,23 @@ // 1.6.2 and earlier fg_set_format(this->fg, VIDEO_PALETTE_YUV420P); this->frame = frame_new(this->width, this->height, VIDEO_PALETTE_YUV420P ); - this->data.format = PLAYER_CAMERA_FORMAT_RGB888; + this->format = PLAYER_CAMERA_FORMAT_RGB888; this->depth = 24; this->rgb_converted_frame = frame_new(this->width, this->height, VIDEO_PALETTE_RGB24 ); - // this->data.format = PLAYER_CAMERA_FORMAT_MONO8; - // this->depth = 8; } else if (strcasecmp(this->palette, "YUV420P_GREY") == 0) { fg_set_format(this->fg, VIDEO_PALETTE_YUV420P); this->frame = frame_new(this->width, this->height, VIDEO_PALETTE_YUV420P ); - this->data.format = PLAYER_CAMERA_FORMAT_MONO8; + this->format = PLAYER_CAMERA_FORMAT_MONO8; this->depth = 8; } else if (strcasecmp(this->palette, "JPEG") == 0) { fg_set_format(this->fg, VIDEO_PALETTE_JPEG ); this->frame = frame_new(this->width, this->height, VIDEO_PALETTE_JPEG ); - this->data.format = PLAYER_CAMERA_FORMAT_RGB888; + this->format = PLAYER_CAMERA_FORMAT_RGB888; this->depth = 24; } else @@ -450,7 +450,7 @@ // Free resources frame_release(this->frame); if ((this->frame->format == VIDEO_PALETTE_YUV420P)&& - (this->data.format == PLAYER_CAMERA_FORMAT_RGB888)) + (this->format == PLAYER_CAMERA_FORMAT_RGB888)) frame_release(this->rgb_converted_frame); fg_close(this->fg); return 0; @@ -510,7 +510,7 @@ //printf("click %d\n", frameno); snprintf(filename, sizeof(filename), "click-%04d.ppm", frameno++); if ((this->frame->format == VIDEO_PALETTE_YUV420P)&& - (this->data.format == PLAYER_CAMERA_FORMAT_RGB888)) + (this->format == PLAYER_CAMERA_FORMAT_RGB888)) { frame_save(this->rgb_converted_frame, filename); //printf("saved converted frame\n"); @@ -528,12 +528,12 @@ void * data) { assert(hdr); - assert(data); /* We currently don't support any messages, but if we do... if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_CAMERA_REQ_GET_GEOM, this->device_addr)) { + assert(data); assert(hdr->size == sizeof(player_position2d_data_t)); ProcessGetGeom(hdr, *reinterpret_cast<player_camera_data_t *> (data)); return(0); @@ -544,60 +544,96 @@ return -1; } +#define IS_JPEG(ptr) ((((ptr)[0]) == 0xff) && (((ptr)[1]) == 0xd8)) + //////////////////////////////////////////////////////////////////////////////// // Update the device data (the data going back to the client). void CameraV4L::RefreshData() { int i; - size_t image_count; unsigned char * ptr1, * ptr2; + player_camera_data_t * data = reinterpret_cast<player_camera_data_t *>(malloc(sizeof(player_camera_data_t))); - // Compute size of image - image_count = this->width * this->height * this->depth / 8; + if (!data) + { + PLAYER_ERROR("Out of memory!"); + return; + } // Set the image properties - this->data.width = this->width; - this->data.height = this->height; - this->data.bpp = this->depth; - this->data.image_count = image_count; - this->data.image = new unsigned char [image_count]; - this->data.compression = PLAYER_CAMERA_COMPRESS_RAW; + data->width = this->width; + data->height = this->height; + data->bpp = this->depth; + data->format = this->format; + data->fdiv = 0; + data->image_count = 0; + data->image = NULL; + data->compression = PLAYER_CAMERA_COMPRESS_RAW; if (have_ov519) { - this->data.image_count = (*(unsigned short *)(frame->data))*8; - assert(data.image_count > 0); - assert(data.image_count <= ((size_t)(this->frame->size))); - this->data.compression = PLAYER_CAMERA_COMPRESS_JPEG; - memcpy(data.image, &(((char*)frame->data)[2]), data.image_count); + data->image_count = (*(reinterpret_cast<unsigned short *>(frame->data))) * 8; + assert(data->image_count > 0); + assert(data->image_count <= static_cast<size_t>(this->frame->size)); + data->image = reinterpret_cast<uint8_t *>(malloc(data->image_count)); + if (!(data->image)) + { + PLAYER_ERROR("Out of memory!"); + free(data); + return; + } + data->compression = PLAYER_CAMERA_COMPRESS_JPEG; + memcpy(data->image, &((reinterpret_cast<char *>(frame->data))[2]), data->image_count); } else if (this->fg->picture.palette == VIDEO_PALETTE_JPEG) { - this->data.compression = PLAYER_CAMERA_COMPRESS_JPEG; memcpy(&i, this->frame->data, sizeof(int)); - data.image_count = i; - assert(data.image_count > 0); - assert(data.image_count <= ((size_t)(this->frame->size))); - memcpy(data.image, ((unsigned char *)(this->frame->data)) + sizeof(int), data.image_count); + data->image_count = i; + assert(data->image_count > 1); + assert(data->image_count <= static_cast<size_t>(this->frame->size)); + if (!(IS_JPEG((reinterpret_cast<unsigned char *>(this->frame->data)) + sizeof(int)))) + { + PLAYER_ERROR("Not an JPEG image..."); + free(data); + return; + } + data->image = reinterpret_cast<uint8_t *>(malloc(data->image_count)); + if (!(data->image)) + { + PLAYER_ERROR("Out of memory!"); + free(data); + return; + } + data->compression = PLAYER_CAMERA_COMPRESS_JPEG; + memcpy(data->image, (reinterpret_cast<unsigned char *>(this->frame->data)) + sizeof(int), data->image_count); } else { + data->image_count = this->width * this->height * this->depth / 8; + assert(data->image_count > 0); + data->image = reinterpret_cast<uint8_t *>(malloc(data->image_count)); + if (!(data->image)) + { + PLAYER_ERROR("Out of memory!"); + free(data); + return; + } // Copy the image pixels if ((this->frame->format == VIDEO_PALETTE_YUV420P) && - (this->data.format == PLAYER_CAMERA_FORMAT_RGB888)) + (this->format == PLAYER_CAMERA_FORMAT_RGB888)) {// do conversion to RGB (which is bgr at the moment for some reason?) - assert(data.image_count <= (size_t) this->rgb_converted_frame->size); + assert(data->image_count <= static_cast<size_t>(this->rgb_converted_frame->size)); ccvt_420p_bgr24(this->width, this->height, - (unsigned char*) this->frame->data, - (unsigned char*) this->rgb_converted_frame->data); - ptr1 = (unsigned char *)this->rgb_converted_frame->data; + reinterpret_cast<unsigned char *>(this->frame->data), + reinterpret_cast<unsigned char *>(this->rgb_converted_frame->data)); + ptr1 = reinterpret_cast<unsigned char *>(this->rgb_converted_frame->data); } else { - assert(data.image_count <= (size_t) this->frame->size); - ptr1 = (unsigned char *)this->frame->data; + assert(data->image_count <= static_cast<size_t>(this->frame->size)); + ptr1 = reinterpret_cast<unsigned char *>(this->frame->data); } - ptr2 = this->data.image; + ptr2 = reinterpret_cast<unsigned char *>(data->image); switch (this->depth) { case 24: @@ -622,7 +658,7 @@ } break; default: - memcpy(ptr2, ptr1, data.image_count); + memcpy(ptr2, ptr1, data->image_count); } } @@ -630,17 +666,19 @@ { if ((time(NULL) - (this->publish_time)) < (this->publish_interval)) { - this->data.width = 0; - this->data.height = 0; - this->data.bpp = 0; - this->data.image_count = 0; + data->width = 0; + data->height = 0; + data->bpp = 0; + data->image_count = 0; + free(data->image); + data->image = NULL; } else this->publish_time = time(NULL); } Publish(this->device_addr, PLAYER_MSGTYPE_DATA, PLAYER_CAMERA_DATA_STATE, - reinterpret_cast<void*>(&this->data)); - delete [] this->data.image; + reinterpret_cast<void *>(data), 0, NULL, false); + // copy = false, don't dispose anything here! return; } Modified: code/player/trunk/server/drivers/laser/lms400_cola.h =================================================================== --- code/player/trunk/server/drivers/laser/lms400_cola.h 2008-10-07 10:27:52 UTC (rev 7074) +++ code/player/trunk/server/drivers/laser/lms400_cola.h 2008-10-07 10:42:15 UTC (rev 7075) @@ -12,6 +12,7 @@ #include <vector> #include <netinet/in.h> #include <libplayercore/player.h> +#include <iostream> #define BUF_SIZE 1024 Modified: code/player/trunk/server/drivers/laser/sicklms200.cc =================================================================== --- code/player/trunk/server/drivers/laser/sicklms200.cc 2008-10-07 10:27:52 UTC (rev 7074) +++ code/player/trunk/server/drivers/laser/sicklms200.cc 2008-10-07 10:42:15 UTC (rev 7075) @@ -1660,43 +1660,6 @@ if(timeout_header == -1) timeout_header = timeout; - // If the timeout is infinite, - // go to blocking io - // - if (timeout < 0) - { - //PLAYER_MSG0(2, "using blocking io"); - int flags = ::fcntl(this->laser_fd, F_GETFL); - if (flags < 0) - { - PLAYER_ERROR("unable to get device flags"); - return 0; - } - if (::fcntl(this->laser_fd, F_SETFL, flags & (~O_NONBLOCK)) < 0) - { - PLAYER_ERROR("unable to set device flags"); - return 0; - } - } - // - // Otherwise, use non-blocking io - // - else - { - //PLAYER_MSG0(2, "using non-blocking io"); - int flags = ::fcntl(this->laser_fd, F_GETFL); - if (flags < 0) - { - PLAYER_ERROR("unable to get device flags"); - return 0; - } - if (::fcntl(this->laser_fd, F_SETFL, flags | O_NONBLOCK) < 0) - { - PLAYER_ERROR("unable to set device flags"); - return 0; - } - } - int64_t start_time = GetTime(); int64_t stop_time = start_time + timeout; int64_t stop_time_header = start_time + timeout_header; @@ -1713,10 +1676,42 @@ while (true) { if (timeout >= 0) - usleep(1000); + { + fd_set rfds, efds; + FD_ZERO(&rfds); + FD_SET(this->laser_fd, &rfds); + FD_ZERO(&efds); + FD_SET(this->laser_fd, &efds); + int64_t delay = stop_time_header - GetTime(); + if (delay < 0) delay = 0; + struct timeval tv; + tv.tv_usec = (delay % 1000) * 1000; + tv.tv_sec = delay / 1000; + int retval = ::select(this->laser_fd + 1, &rfds, 0, &efds, &tv); + if (retval < 0) + { + PLAYER_ERROR("error on select (1)"); + return 0; + } + if (!retval) + { + PLAYER_MSG0(2, "timeout on select (1)"); + return 0; + } + } //printf("reading %d\n", timeout); fflush(stdout); bytes = ::read(this->laser_fd, header + sizeof(header) - 1, 1); + if (bytes < 0) + { + PLAYER_ERROR("error on read (1)"); + return 0; + } + if (!bytes) + { + PLAYER_MSG0(2, "eof on read (1)"); + return 0; + } //printf("bytes read %d\n", bytes); fflush(stdout); if (header[0] == STX && header[1] == 0x80) @@ -1727,12 +1722,6 @@ break; } memmove(header, header + 1, sizeof(header) - 1); - if (timeout >= 0 && GetTime() >= stop_time_header) - { - //PLAYER_MSG2(2, "%Ld %Ld", GetTime(), stop_time); - PLAYER_MSG0(2, "timeout on read (1)"); - return 0; - } } // Determine data length @@ -1754,13 +1743,41 @@ while (bytes < len) { if (timeout >= 0) - usleep(1000); - bytes += ::read(this->laser_fd, data + bytes, len - bytes); - if (timeout >= 0 && GetTime() >= stop_time) { - //PLAYER_MSG2(2, "%Ld %Ld", GetTime(), stop_time); - RETURN_ERROR(0, "timeout on read (3)"); + fd_set rfds, efds; + FD_ZERO(&rfds); + FD_SET(this->laser_fd, &rfds); + FD_ZERO(&efds); + FD_SET(this->laser_fd, &efds); + int64_t delay = stop_time - GetTime(); + if (delay < 0) delay = 0; + struct timeval tv; + tv.tv_usec = (delay % 1000) * 1000; + tv.tv_sec = delay / 1000; + int retval = ::select(this->laser_fd + 1, &rfds, 0, &efds, &tv); + if (retval < 0) + { + PLAYER_ERROR("error on select (3)"); + return 0; + } + if (!retval) + { + PLAYER_MSG0(2, "timeout on select (3)"); + return 0; + } } + int retval = ::read(this->laser_fd, data + bytes, len - bytes); + if (retval < 0) + { + PLAYER_ERROR("error on read (3)"); + return 0; + } + if (!retval) + { + PLAYER_MSG0(2, "eof on read (3)"); + return 0; + } + bytes += retval; } // Read in footer @@ -1769,13 +1786,41 @@ while (bytes < 3) { if (timeout >= 0) - usleep(1000); - bytes += ::read(this->laser_fd, footer + bytes, 3 - bytes); - if (timeout >= 0 && GetTime() >= stop_time) { - //PLAYER_MSG2(2, "%Ld %Ld", GetTime(), stop_time); - RETURN_ERROR(0, "timeout on read (4)"); + fd_set rfds, efds; + FD_ZERO(&rfds); + FD_SET(this->laser_fd, &rfds); + FD_ZERO(&efds); + FD_SET(this->laser_fd, &efds); + int64_t delay = stop_time - GetTime(); + if (delay < 0) delay = 0; + struct timeval tv; + tv.tv_usec = (delay % 1000) * 1000; + tv.tv_sec = delay / 1000; + int retval = ::select(this->laser_fd + 1, &rfds, 0, &efds, &tv); + if (retval < 0) + { + PLAYER_ERROR("error on select (4)"); + return 0; + } + if (!retval) + { + PLAYER_MSG0(2, "timeout on select (4)"); + return 0; + } } + int retval = ::read(this->laser_fd, footer + bytes, 3 - bytes); + if (retval < 0) + { + PLAYER_ERROR("error on read (4)"); + return 0; + } + if (!retval) + { + PLAYER_MSG0(2, "eof on read (4)"); + return 0; + } + bytes += retval; } // Construct entire packet Modified: code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_driver.cc =================================================================== --- code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_driver.cc 2008-10-07 10:27:52 UTC (rev 7074) +++ code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_driver.cc 2008-10-07 10:42:15 UTC (rev 7075) @@ -325,6 +325,7 @@ posdata.pos.px = this->roomba_dev->ox; posdata.pos.py = this->roomba_dev->oy; posdata.pos.pa = this->roomba_dev->oa; + posdata.stall = static_cast<uint8_t>(this->bumplocked); this->Publish(this->position_addr, PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE, Modified: code/player/trunk/server/drivers/mixed/mricp/src/geometry2D.cpp =================================================================== --- code/player/trunk/server/drivers/mixed/mricp/src/geometry2D.cpp 2008-10-07 10:27:52 UTC (rev 7074) +++ code/player/trunk/server/drivers/mixed/mricp/src/geometry2D.cpp 2008-10-07 10:42:15 UTC (rev 7075) @@ -6,6 +6,7 @@ #include <cmath> #include <cassert> #include <stdio.h> +#include <iostream> using namespace std; Modified: code/player/trunk/server/drivers/opaque/serialstream.cc =================================================================== --- code/player/trunk/server/drivers/opaque/serialstream.cc 2008-10-07 10:27:52 UTC (rev 7074) +++ code/player/trunk/server/drivers/opaque/serialstream.cc 2008-10-07 10:42:15 UTC (rev 7075) @@ -61,6 +61,9 @@ - buffer_size (integer - The size of the buffer to be used when reading, this is the maximum that can be read in one read command - Default 4096 + +- parity + - The parity that you want. Vaid vaules are "none" (default), "even", "odd" @par Example @@ -390,7 +393,9 @@ } // save the current io settings - tcgetattr(opaque_fd, &oldtio); + if (tcgetattr(opaque_fd, &oldtio) < 0) { + PLAYER_WARN("Failed to get old tty attributes"); + } // set up new settings UpdateFlags(); @@ -416,7 +421,13 @@ // set up new settings struct termios newtio; memset(&newtio, 0,sizeof(newtio)); - newtio.c_cflag = CS8 | CREAD; + + if(tcgetattr( this->opaque_fd, &newtio ) < 0 ) + PLAYER_WARN("Failed to get old tty attribltes"); + + newtio.c_cc[VMIN] = 0; + newtio.c_cc[VTIME] = 0; + newtio.c_cflag = CS8 | CREAD; newtio.c_iflag = INPCK; newtio.c_oflag = 0; newtio.c_lflag = 0; @@ -428,8 +439,9 @@ newtio.c_cflag |= PARENB | PARODD; else PLAYER_WARN("Invalid parity. Defaulting to none."); - - tcsetattr(opaque_fd, TCSANOW, &newtio); + + if(tcsetattr(opaque_fd, TCSANOW, &newtio) < 0) + PLAYER_ERROR("Failed to set new tty device attributes"); tcflush(opaque_fd, TCIOFLUSH); } Modified: code/player/trunk/server/drivers/rfid/insideM300.cc =================================================================== --- code/player/trunk/server/drivers/rfid/insideM300.cc 2008-10-07 10:27:52 UTC (rev 7074) +++ code/player/trunk/server/drivers/rfid/insideM300.cc 2008-10-07 10:42:15 UTC (rev 7075) @@ -22,6 +22,7 @@ Author: Radu Bogdan Rusu Date: 25 Jan 2006 CVS: $Id$ + Fixes by Alexis Maldonado <maldonad //at// cs.tum.edu>. April/2008. */ /** @ingroup drivers */ @@ -225,7 +226,6 @@ // Destructor. InsideM300::~InsideM300() { - free(Data.tags); } //////////////////////////////////////////////////////////////////////////////// @@ -380,6 +380,14 @@ tcsetattr (this->fd, TCSANOW, &this->initial_options); close (this->fd); + for (unsigned int i=0; i != allocated_tags; ++i) { + delete [] Data.tags[i].guid; + } + + free(Data.tags); + Data.tags=NULL; + allocated_tags=0; + PLAYER_MSG0 (1, "> InsideM300 driver shutting down... [done]"); return (0); } @@ -459,7 +467,7 @@ // RefreshData function void InsideM300::RefreshData () { - memset (&this->Data, 0, sizeof (player_rfid_data_t)); + memset (&this->Data, 0, sizeof (Data)); // Get the time at which we started reading // This will be a pretty good estimate of when the phenomena occured @@ -680,10 +688,12 @@ if (this->Data.tags_count >= this->allocated_tags) { this->allocated_tags = this->Data.tags_count+1; - this->Data.tags = (player_rfid_tag_t*)realloc(this->Data.tags,sizeof(this->Data.tags[0])*this->allocated_tags); + this->Data.tags = (player_rfid_tag_t*)realloc(this->Data.tags,sizeof(player_rfid_tag_t)*this->allocated_tags); + this->Data.tags[this->Data.tags_count].guid_count = 8; + this->Data.tags[this->Data.tags_count].guid=new char[8]; + } this->Data.tags[this->Data.tags_count].type = chipAnswer[1]; - this->Data.tags[this->Data.tags_count].guid_count = 8; int j; for (j = 0; j < 8; j++) this->Data.tags[this->Data.tags_count].guid[j] = @@ -760,10 +770,11 @@ if (this->Data.tags_count >= this->allocated_tags) { this->allocated_tags = this->Data.tags_count+1; - this->Data.tags = (player_rfid_tag_t*)realloc(this->Data.tags,sizeof(this->Data.tags[0])*this->allocated_tags); + this->Data.tags = (player_rfid_tag_t*)realloc(this->Data.tags,sizeof(player_rfid_tag_t)*this->allocated_tags); + this->Data.tags[this->Data.tags_count].guid_count = 8; + this->Data.tags[this->Data.tags_count].guid=new char[8]; } this->Data.tags[this->Data.tags_count].type = chipAnswer[1]; - this->Data.tags[this->Data.tags_count].guid_count = 8; int j; for (j = 0; j < 8; j++) this->Data.tags[this->Data.tags_count].guid[j] = chipAnswer [9-j]; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2008-10-07 20:08:28
|
Revision: 7086 http://playerstage.svn.sourceforge.net/playerstage/?rev=7086&view=rev Author: thjc Date: 2008-10-07 20:08:21 +0000 (Tue, 07 Oct 2008) Log Message: ----------- make the .player log file optional and configurable Modified Paths: -------------- code/player/trunk/libplayercore/bindings/java/main.java code/player/trunk/libplayercore/error.c code/player/trunk/libplayercore/error.h code/player/trunk/libplayersd/playersd_mdns.c code/player/trunk/libplayertcp/bindings/java/main.java code/player/trunk/libplayertcp/playertcp.cc code/player/trunk/server/drivers/mixed/wbr/914/wbr914.cc code/player/trunk/server/server.cc Modified: code/player/trunk/libplayercore/bindings/java/main.java =================================================================== --- code/player/trunk/libplayercore/bindings/java/main.java 2008-10-07 11:42:58 UTC (rev 7085) +++ code/player/trunk/libplayercore/bindings/java/main.java 2008-10-07 20:08:21 UTC (rev 7086) @@ -54,7 +54,7 @@ // Initialization stuff playercore_java.player_register_drivers(); - playercore_java.ErrorInit(9); + playercore_java.ErrorInit(9,NULL); ConfigFile cf = new ConfigFile(0, 0); Modified: code/player/trunk/libplayercore/error.c =================================================================== --- code/player/trunk/libplayercore/error.c 2008-10-07 11:42:58 UTC (rev 7085) +++ code/player/trunk/libplayercore/error.c 2008-10-07 20:08:21 UTC (rev 7086) @@ -38,10 +38,10 @@ // Initialize error logging void -ErrorInit(int _msgLevel) +ErrorInit(int _msgLevel, FILE * logfile) { msgLevel = _msgLevel; - msgFile = fopen(".player", "a+"); + msgFile = logfile; } #define MSG_MAX 1024 Modified: code/player/trunk/libplayercore/error.h =================================================================== --- code/player/trunk/libplayercore/error.h 2008-10-07 11:42:58 UTC (rev 7085) +++ code/player/trunk/libplayercore/error.h 2008-10-07 20:08:21 UTC (rev 7086) @@ -34,7 +34,7 @@ #endif /// @internal Initialize error logging -void ErrorInit(int _msgLevel); +void ErrorInit(int _msgLevel, FILE * logfile); /// @internal Function for print and logging errors. Do not call this /// function directly; use the macros below. Modified: code/player/trunk/libplayersd/playersd_mdns.c =================================================================== --- code/player/trunk/libplayersd/playersd_mdns.c 2008-10-07 11:42:58 UTC (rev 7085) +++ code/player/trunk/libplayersd/playersd_mdns.c 2008-10-07 20:08:21 UTC (rev 7086) @@ -163,7 +163,7 @@ itable_init(); // Pick a default debug level - ErrorInit(1); + ErrorInit(1,NULL); return(sd); } Modified: code/player/trunk/libplayertcp/bindings/java/main.java =================================================================== --- code/player/trunk/libplayertcp/bindings/java/main.java 2008-10-07 11:42:58 UTC (rev 7085) +++ code/player/trunk/libplayertcp/bindings/java/main.java 2008-10-07 20:08:21 UTC (rev 7086) @@ -58,7 +58,7 @@ // Initialization stuff playercore_java.player_register_drivers(); - playercore_java.ErrorInit(9); + playercore_java.ErrorInit(9,NULL); playertcp_java.playerxdr_ftable_init(); // Convert "localhost" to a 32-bit address. It will be used to Modified: code/player/trunk/libplayertcp/playertcp.cc =================================================================== --- code/player/trunk/libplayertcp/playertcp.cc 2008-10-07 11:42:58 UTC (rev 7085) +++ code/player/trunk/libplayertcp/playertcp.cc 2008-10-07 20:08:21 UTC (rev 7086) @@ -114,7 +114,7 @@ PlayerTCP::InitGlobals(void) { player_globals_init(); - ErrorInit(1); + ErrorInit(1,NULL); playerxdr_ftable_init(); } Modified: code/player/trunk/server/drivers/mixed/wbr/914/wbr914.cc =================================================================== --- code/player/trunk/server/drivers/mixed/wbr/914/wbr914.cc 2008-10-07 11:42:58 UTC (rev 7085) +++ code/player/trunk/server/drivers/mixed/wbr/914/wbr914.cc 2008-10-07 20:08:21 UTC (rev 7086) @@ -157,7 +157,7 @@ // Baud rate _baud = 416666; - ErrorInit( 9 ); + ErrorInit( 9, NULL ); // zero ids, so that we'll know later which interfaces were // requested Modified: code/player/trunk/server/server.cc =================================================================== --- code/player/trunk/server/server.cc 2008-10-07 11:42:58 UTC (rev 7085) +++ code/player/trunk/server/server.cc 2008-10-07 20:08:21 UTC (rev 7086) @@ -62,6 +62,7 @@ - -p \<port\> : Establish the default TCP port, which will be assigned to any devices in the configuration file without an explicit port assignment. Default: 6665. +- -l <logfile>: File to log messages to (default stdout only) - \<cfgfile\> : The configuration file to read. @section Example @@ -110,7 +111,7 @@ void PrintCopyrightMsg(); void PrintUsage(); int ParseArgs(int* port, int* debuglevel, - char** cfgfilename, int* gz_serverid, + char** cfgfilename, int* gz_serverid, char** logfilename, int argc, char** argv); void Quit(int signum); void Cleanup(); @@ -133,6 +134,7 @@ int* new_ports; int num_ports; char* cfgfilename; + char * logfileName = NULL; if(signal(SIGINT, Quit) == SIG_ERR) { @@ -158,13 +160,16 @@ assert(pudp); PrintVersion(); - if(ParseArgs(&port, &debuglevel, &cfgfilename, &gz_serverid, argc, argv) < 0) + if(ParseArgs(&port, &debuglevel, &cfgfilename, &gz_serverid, &logfileName, argc, argv) < 0) { PrintUsage(); exit(-1); } - ErrorInit(debuglevel); + FILE* logfile = NULL; + if (logfileName) + logfile = fopen(logfileName,"a"); + ErrorInit(debuglevel,logfile); PrintCopyrightMsg(); @@ -399,7 +404,7 @@ int -ParseArgs(int* port, int* debuglevel, char** cfgfilename, int* gz_serverid, +ParseArgs(int* port, int* debuglevel, char** cfgfilename, int* gz_serverid, char **logfilename, int argc, char** argv) { int ch; @@ -416,6 +421,9 @@ case 'd': *debuglevel = atoi(optarg); break; + case 'l': + *logfilename = optarg; + break; case 'p': *port = atoi(optarg); break; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <ge...@us...> - 2008-10-16 07:48:57
|
Revision: 7100 http://playerstage.svn.sourceforge.net/playerstage/?rev=7100&view=rev Author: gerkey Date: 2008-10-16 07:48:50 +0000 (Thu, 16 Oct 2008) Log Message: ----------- Fixed dependencies when building a new interface Modified Paths: -------------- code/player/trunk/libplayercore/CMakeLists.txt code/player/trunk/libplayerxdr/functiontable.c Modified: code/player/trunk/libplayercore/CMakeLists.txt =================================================================== --- code/player/trunk/libplayercore/CMakeLists.txt 2008-10-15 16:01:14 UTC (rev 7099) +++ code/player/trunk/libplayercore/CMakeLists.txt 2008-10-16 07:48:50 UTC (rev 7100) @@ -6,25 +6,11 @@ MESSAGE (FATAL_ERROR "No Python interpreter found. Cannot continue.") ENDIF (NOT PYTHONINTERP_FOUND) -SET (interfaceDefs interfaces/001_player.def interfaces/002_power.def interfaces/003_gripper.def - interfaces/004_position2d.def interfaces/005_sonar.def interfaces/006_laser.def - interfaces/007_blobfinder.def interfaces/008_ptz.def interfaces/009_audio.def - interfaces/010_fiducial.def interfaces/012_speech.def interfaces/013_gps.def - interfaces/014_bumper.def interfaces/020_dio.def interfaces/021_aio.def - interfaces/022_ir.def interfaces/023_wifi.def interfaces/025_localize.def - interfaces/026_mcom.def interfaces/030_position3d.def - interfaces/031_simulation.def interfaces/033_blinkenlight.def - interfaces/040_camera.def interfaces/042_map.def interfaces/044_planner.def - interfaces/045_log.def interfaces/049_joystick.def - interfaces/050_speech_recognition.def interfaces/051_opaque.def - interfaces/052_position1d.def interfaces/053_actarray.def - interfaces/054_limb.def interfaces/055_graphics2d.def interfaces/056_rfid.def - interfaces/057_wsn.def interfaces/058_graphics3d.def interfaces/059_health.def - interfaces/060_imu.def interfaces/061_pointcloud3d.def interfaces/062_ranger.def - interfaces/063_vectormap.def interfaces/064_blackboard.def) +FILE (GLOB interfaceFiles ${PROJECT_SOURCE_DIR}/libplayercore/interfaces/*.def) + SET (player_interfaces_h "${CMAKE_CURRENT_BINARY_DIR}/player_interfaces.h") SET (interface_table_h "${CMAKE_CURRENT_BINARY_DIR}/interface_table.h") -PROCESS_INTERFACES ("" ${CMAKE_CURRENT_SOURCE_DIR}/interfaces ${player_interfaces_h} ${interfaceDefs}) +PROCESS_INTERFACES ("" ${CMAKE_CURRENT_SOURCE_DIR}/interfaces ${player_interfaces_h} ${interfaceFiles}) ADD_CUSTOM_TARGET (player_interfaces ALL DEPENDS ${player_interfaces_h} COMMENT "Generating player_interfaces.h") Modified: code/player/trunk/libplayerxdr/functiontable.c =================================================================== --- code/player/trunk/libplayerxdr/functiontable.c 2008-10-15 16:01:14 UTC (rev 7099) +++ code/player/trunk/libplayerxdr/functiontable.c 2008-10-16 07:48:50 UTC (rev 7100) @@ -53,7 +53,7 @@ // dependency //#include <libplayercore/error.h> -#include "playerxdr.h" +#include "libplayerxdr/playerxdr.h" #include "functiontable.h" #ifndef HAVE_XDR_LONGLONG_T This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <ge...@us...> - 2008-10-16 09:01:22
|
Revision: 7101 http://playerstage.svn.sourceforge.net/playerstage/?rev=7101&view=rev Author: gerkey Date: 2008-10-16 08:57:01 +0000 (Thu, 16 Oct 2008) Log Message: ----------- Commented out bogus warning about empty queue after waiting. Fixed apparent bug in amcl's data queuing. Modified Paths: -------------- code/player/trunk/libplayercore/device.cc code/player/trunk/server/drivers/localization/amcl/amcl.cc Modified: code/player/trunk/libplayercore/device.cc =================================================================== --- code/player/trunk/libplayercore/device.cc 2008-10-16 07:48:50 UTC (rev 7100) +++ code/player/trunk/libplayercore/device.cc 2008-10-16 08:57:01 UTC (rev 7101) @@ -288,12 +288,11 @@ Message* msg = NULL; if(threaded) { - // HACK: this loop should not be neccesary! // pthread_cond_wait does not garuntee no false wake up, so maybe it is. // test driver is still subscribed to prevent deadlocks on server shutdown while(driver->subscriptions > 0 && !(msg = resp_queue->Pop())) { - PLAYER_WARN("empty queue after waiting!"); + //PLAYER_WARN("empty queue after waiting!"); resp_queue->Wait(); // this is a cancelation point } } Modified: code/player/trunk/server/drivers/localization/amcl/amcl.cc =================================================================== --- code/player/trunk/server/drivers/localization/amcl/amcl.cc 2008-10-16 07:48:50 UTC (rev 7100) +++ code/player/trunk/server/drivers/localization/amcl/amcl.cc 2008-10-16 08:57:01 UTC (rev 7101) @@ -877,6 +877,9 @@ { data = this->Pop(); assert(data); + // Catch the pose and timestamp of the discarded action data + pose = ((AMCLOdomData*) data)->pose; + ts = data->time; delete data; } // avoid a busy loop while waiting for a sensor reading to @@ -1324,7 +1327,11 @@ pf_vector_t pose; this->Lock(); + pthread_mutex_lock(&this->best_hyp_lock); + pose = this->best_hyp; + pthread_mutex_unlock(&this->best_hyp_lock); + /* max_weight = -1; for (i = 0; i < this->hyp_count; i++) { @@ -1336,11 +1343,14 @@ pose = hyp->pf_pose_mean; } } + */ this->Unlock(); + /* if (max_weight < 0.0) return; + */ // Shift the robot figure rtk_fig_origin(this->robot_fig, pose.v[0], pose.v[1], pose.v[2]); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2008-12-18 19:58:41
|
Revision: 7206 http://playerstage.svn.sourceforge.net/playerstage/?rev=7206&view=rev Author: thjc Date: 2008-12-18 19:58:30 +0000 (Thu, 18 Dec 2008) Log Message: ----------- Merged assert fixes from 7204 also some cmake fixes for postgis/vec2map drivers Modified Paths: -------------- code/player/trunk/server/drivers/base/imagebase.cc code/player/trunk/server/drivers/camera/1394/camera1394.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/sphere/sphere_mixed.cc code/player/trunk/server/drivers/camera/v4l/camerav4l.cc code/player/trunk/server/drivers/fiducial/laserbar.cc code/player/trunk/server/drivers/fiducial/laserbarcode.cc code/player/trunk/server/drivers/fiducial/laservisualbarcode.cc code/player/trunk/server/drivers/fiducial/laservisualbw.cc code/player/trunk/server/drivers/imu/XSensMT.cc code/player/trunk/server/drivers/imu/nimuplayer.cpp code/player/trunk/server/drivers/laser/sicklms400.cc code/player/trunk/server/drivers/map/maptransform.cc code/player/trunk/server/drivers/mcom/lifomcom.cc code/player/trunk/server/drivers/mixed/clodbuster/clodbuster.cc code/player/trunk/server/drivers/mixed/cmucam2/cmucam2.cc code/player/trunk/server/drivers/mixed/evolution/er1/er.cc code/player/trunk/server/drivers/mixed/garcia/garcia_mixed.cc code/player/trunk/server/drivers/mixed/khepera/khepera.cc code/player/trunk/server/drivers/mixed/reb/reb.cc code/player/trunk/server/drivers/mixed/sr3000/sr3000.cc code/player/trunk/server/drivers/position/ascension/flockofbirds.cc code/player/trunk/server/drivers/position/bumpersafe/bumpersafe.cc code/player/trunk/server/drivers/position/isense/inertiacube2.cc code/player/trunk/server/drivers/position/lasersafe/lasersafe.cc code/player/trunk/server/drivers/position/mbicp/mbicp_driver.cc code/player/trunk/server/drivers/position/roboteq/roboteq.cc code/player/trunk/server/drivers/ptz/amtecpowercube.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/rfid/sickrfi341.cc code/player/trunk/server/drivers/speech/festival.cc code/player/trunk/server/drivers/vectormap/CMakeLists.txt code/player/trunk/server/drivers/vectormap/vec2map.cc code/player/trunk/server/drivers/wifi/linuxwifi.cc code/player/trunk/server/drivers/wsn/accel_calib.cc code/player/trunk/server/drivers/wsn/mica2.cc code/player/trunk/server/drivers/wsn/rcore_xbridge.cc Property Changed: ---------------- code/player/trunk/ Property changes on: code/player/trunk ___________________________________________________________________ Modified: svn:mergeinfo - /code/player/branches/release-2-1-patches:6672-6673,6738,6834,6886,6905,6930,6936,6946,6951,6955,6975,6979-6981,6985,6987-6996 /code/player/trunk:6985 + /code/player/branches/release-2-1-patches:6672-6673,6738,6834,6886,6905,6930,6936,6946,6951,6955,6975,6979-6981,6985,6987-6996,7204 /code/player/trunk:6985 Modified: code/player/trunk/server/drivers/base/imagebase.cc =================================================================== --- code/player/trunk/server/drivers/base/imagebase.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/base/imagebase.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -120,13 +120,13 @@ int ImageBase::ProcessMessage (QueuePointer &resp_queue, player_msghdr * hdr, void * data) { uint32_t new_image_count; - player_camera_data_t * compdata = reinterpret_cast<player_camera_data_t *>(data); assert(hdr); - assert(compdata); if(Message::MatchMessage (hdr, PLAYER_MSGTYPE_DATA, PLAYER_CAMERA_DATA_STATE, camera_addr)) { + assert(data); + player_camera_data_t * compdata = reinterpret_cast<player_camera_data_t *>(data); Lock(); if (!HaveData) { Modified: code/player/trunk/server/drivers/camera/1394/camera1394.cc =================================================================== --- code/player/trunk/server/drivers/camera/1394/camera1394.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/camera/1394/camera1394.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -1157,7 +1157,6 @@ void * data) { assert(hdr); - assert(data); /* We currently don't support any messages, but if we do... if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, Modified: code/player/trunk/server/drivers/camera/compress/cameracompress.cc =================================================================== --- code/player/trunk/server/drivers/camera/compress/cameracompress.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/camera/compress/cameracompress.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -204,10 +204,10 @@ void * data) { assert(hdr); - assert(data); if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA, PLAYER_CAMERA_DATA_STATE, camera_id)) { + assert(data); player_camera_data_t * recv = reinterpret_cast<player_camera_data_t * > (data); ProcessImage(*recv); return 0; Modified: code/player/trunk/server/drivers/camera/compress/camerauncompress.cc =================================================================== --- code/player/trunk/server/drivers/camera/compress/camerauncompress.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/camera/compress/camerauncompress.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -198,10 +198,10 @@ void * data) { assert(hdr); - assert(data); if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA, PLAYER_CAMERA_DATA_STATE, camera_id)) { + assert(data); if(hdr->size < sizeof(player_camera_data_t)) { PLAYER_WARN("Not enough camera data recieved"); Modified: code/player/trunk/server/drivers/camera/sphere/sphere_mixed.cc =================================================================== --- code/player/trunk/server/drivers/camera/sphere/sphere_mixed.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/camera/sphere/sphere_mixed.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -419,7 +419,6 @@ void* data) { assert(hdr); - assert(data); if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_PTZ_CMD_STATE, mPtzAddr)) Modified: code/player/trunk/server/drivers/camera/v4l/camerav4l.cc =================================================================== --- code/player/trunk/server/drivers/camera/v4l/camerav4l.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/camera/v4l/camerav4l.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -533,8 +533,6 @@ if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_CAMERA_REQ_GET_GEOM, this->device_addr)) { - assert(data); - assert(hdr->size == sizeof(player_position2d_data_t)); ProcessGetGeom(hdr, *reinterpret_cast<player_camera_data_t *> (data)); return(0); Modified: code/player/trunk/server/drivers/fiducial/laserbar.cc =================================================================== --- code/player/trunk/server/drivers/fiducial/laserbar.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/fiducial/laserbar.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -287,7 +287,6 @@ if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA, PLAYER_LASER_DATA_SCAN, this->laser_addr)) { - //assert(hdr->size == sizeof(player_laser_data_t)); player_laser_data_t *laser_data = reinterpret_cast<player_laser_data_t * > (data); this->Lock(); Modified: code/player/trunk/server/drivers/fiducial/laserbarcode.cc =================================================================== --- code/player/trunk/server/drivers/fiducial/laserbarcode.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/fiducial/laserbarcode.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -278,11 +278,9 @@ int LaserBarcode::ProcessMessage (QueuePointer &resp_queue, player_msghdr * hdr, void * data) { assert(hdr); - assert(data); if(Message::MatchMessage (hdr, PLAYER_MSGTYPE_DATA, PLAYER_LASER_DATA_SCAN, laser_id)) { - assert(hdr->size == sizeof(player_laser_data_t)); laser_data = *reinterpret_cast<player_laser_data_t * > (data); // Analyse the laser data Modified: code/player/trunk/server/drivers/fiducial/laservisualbarcode.cc =================================================================== --- code/player/trunk/server/drivers/fiducial/laservisualbarcode.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/fiducial/laservisualbarcode.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -412,11 +412,9 @@ int LaserVisualBarcode::ProcessMessage (QueuePointer &resp_queue, player_msghdr * hdr, void * data) { assert(hdr); - assert(data); if(Message::MatchMessage (hdr, PLAYER_MSGTYPE_DATA, PLAYER_LASER_DATA_SCAN, laser_id)) { - assert(hdr->size == sizeof(player_laser_data_t)); player_laser_data_t * l_data = reinterpret_cast<player_laser_data_t * > (data); UpdateLaser(l_data, hdr->timestamp); @@ -426,14 +424,12 @@ if(Message::MatchMessage (hdr, PLAYER_MSGTYPE_DATA, PLAYER_PTZ_DATA_STATE, ptz_id)) { - assert(hdr->size == sizeof(player_ptz_data_t)); UpdatePtz(reinterpret_cast<player_ptz_data_t * > (data), hdr->timestamp); return 0; } if(Message::MatchMessage (hdr, PLAYER_MSGTYPE_DATA, PLAYER_BLOBFINDER_DATA_BLOBS, blobfinder_id)) { - assert(hdr->size == sizeof(player_blobfinder_data_t)); UpdateBlobfinder(reinterpret_cast<player_blobfinder_data_t * > (data), hdr->timestamp); return 0; } Modified: code/player/trunk/server/drivers/fiducial/laservisualbw.cc =================================================================== --- code/player/trunk/server/drivers/fiducial/laservisualbw.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/fiducial/laservisualbw.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -394,11 +394,9 @@ int LaserVisualBW::ProcessMessage (QueuePointer &resp_queue, player_msghdr * hdr, void * data) { assert(hdr); - assert(data); if(Message::MatchMessage (hdr, PLAYER_MSGTYPE_DATA, PLAYER_LASER_DATA_SCAN, laser_id)) { - assert(hdr->size == sizeof(player_laser_data_t)); player_laser_data_t * l_data = reinterpret_cast<player_laser_data_t * > (data); UpdateLaser(l_data, hdr->timestamp); @@ -408,14 +406,12 @@ if(Message::MatchMessage (hdr, PLAYER_MSGTYPE_DATA, PLAYER_PTZ_DATA_STATE, ptz_id)) { - assert(hdr->size == sizeof(player_ptz_data_t)); UpdatePtz(reinterpret_cast<player_ptz_data_t * > (data), hdr->timestamp); return 0; } if(Message::MatchMessage (hdr, PLAYER_MSGTYPE_DATA, PLAYER_CAMERA_DATA_STATE, camera_id)) { - assert(hdr->size == sizeof(player_camera_data_t)); UpdateCamera(reinterpret_cast<player_camera_data_t * > (data), hdr->timestamp); return 0; } Modified: code/player/trunk/server/drivers/imu/XSensMT.cc =================================================================== --- code/player/trunk/server/drivers/imu/XSensMT.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/imu/XSensMT.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -278,7 +278,6 @@ void * data) { assert (hdr); - assert (data); // this holds possible error messages returned by mtcomm.writeMessage int err; Modified: code/player/trunk/server/drivers/imu/nimuplayer.cpp =================================================================== --- code/player/trunk/server/drivers/imu/nimuplayer.cpp 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/imu/nimuplayer.cpp 2008-12-18 19:58:30 UTC (rev 7206) @@ -249,7 +249,6 @@ void * data) { assert (hdr); - assert (data); return -1; } Modified: code/player/trunk/server/drivers/laser/sicklms400.cc =================================================================== --- code/player/trunk/server/drivers/laser/sicklms400.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/laser/sicklms400.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -391,7 +391,6 @@ void* data) { assert (hdr); -// assert (data); // ---[ Get geometry if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, Modified: code/player/trunk/server/drivers/map/maptransform.cc =================================================================== --- code/player/trunk/server/drivers/map/maptransform.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/map/maptransform.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -199,8 +199,6 @@ PLAYER_MSG0(9,"ProcessMessage called for MapTransform Driver"); assert(hdr); - // assert fails on empty messages - //assert(data); if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_MAP_REQ_GET_INFO, device_addr)) { Modified: code/player/trunk/server/drivers/mcom/lifomcom.cc =================================================================== --- code/player/trunk/server/drivers/mcom/lifomcom.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/mcom/lifomcom.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -124,8 +124,6 @@ int LifoMCom::ProcessMessage(ClientData * client, player_msghdr * hdr, uint8_t * data, uint8_t * resp_data, size_t * resp_len) { assert(hdr); - assert(data); - assert(resp_len); if (hdr->size < sizeof(player_mcom_config_t)) return -1; Modified: code/player/trunk/server/drivers/mixed/clodbuster/clodbuster.cc =================================================================== --- code/player/trunk/server/drivers/mixed/clodbuster/clodbuster.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/mixed/clodbuster/clodbuster.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -255,7 +255,6 @@ { assert(hdr); - assert(data); if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_SET_ODOM, device_addr)) { Modified: code/player/trunk/server/drivers/mixed/cmucam2/cmucam2.cc =================================================================== --- code/player/trunk/server/drivers/mixed/cmucam2/cmucam2.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/mixed/cmucam2/cmucam2.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -309,7 +309,6 @@ void * data) { assert(hdr); - assert(data); if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_PTZ_CMD_STATE, ptz_id)) { Modified: code/player/trunk/server/drivers/mixed/evolution/er1/er.cc =================================================================== --- code/player/trunk/server/drivers/mixed/evolution/er1/er.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/mixed/evolution/er1/er.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -790,7 +790,6 @@ void * data) { assert(hdr); - assert(data); if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_GET_GEOM, position_id)) { Modified: code/player/trunk/server/drivers/mixed/garcia/garcia_mixed.cc =================================================================== --- code/player/trunk/server/drivers/mixed/garcia/garcia_mixed.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/mixed/garcia/garcia_mixed.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -357,9 +357,7 @@ player_msghdr* hdr, void* data) { - assert(resp_queue); assert(hdr); - assert(data); if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_POS, mPos2dAddr)) @@ -378,14 +376,12 @@ else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_SPEECH_CMD_SAY, mSpeechAddr)) { - assert(hdr->size == sizeof(player_speech_cmd_t)); ProcessSpeechCommand(hdr, *reinterpret_cast<player_speech_cmd_t *>(data)); return(0); } else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA, PLAYER_DIO_CMD_VALUES, mDioAddr)) { - assert(hdr->size == sizeof(player_dio_cmd_t)); ProcessDioCommand(hdr, *reinterpret_cast<player_dio_cmd_t *>(data)); return(0); } Modified: code/player/trunk/server/drivers/mixed/khepera/khepera.cc =================================================================== --- code/player/trunk/server/drivers/mixed/khepera/khepera.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/mixed/khepera/khepera.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -162,7 +162,6 @@ int Khepera::ProcessMessage(QueuePointer & resp_queue, player_msghdr * hdr, void * data) { assert(hdr); - assert(data); if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_IR_REQ_POSE, ir_addr)) { Modified: code/player/trunk/server/drivers/mixed/reb/reb.cc =================================================================== --- code/player/trunk/server/drivers/mixed/reb/reb.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/mixed/reb/reb.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -738,9 +738,6 @@ int REB::ProcessMessage(ClientData * client, player_msghdr * hdr, uint8_t * data, uint8_t * resp_data, size_t * resp_len) { assert(hdr); - assert(data); - assert(resp_data); - assert(resp_len); if (MatchMessage(hdr, PLAYER_MSGTYPE_CMD, 0, position_id)) { @@ -752,7 +749,6 @@ if (MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_IR_REQ_POWER, ir_id)) { - assert(hdr->size == sizeof(player_ir_power_req_t)); player_ir_power_req_t * powreq = reinterpret_cast<player_ir_power_req_t *> (data); if (powreq->state) @@ -766,7 +762,6 @@ if (MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_IR_REQ_POSE, ir_id)) { - assert(*resp_len >= sizeof(player_ir_pose_t)); player_ir_pose_t & irpose = *reinterpret_cast<player_ir_pose_t *> (resp_data); uint16_t numir = PlayerUBotRobotParams[param_index].NumberIRSensors; Modified: code/player/trunk/server/drivers/mixed/sr3000/sr3000.cc =================================================================== --- code/player/trunk/server/drivers/mixed/sr3000/sr3000.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/mixed/sr3000/sr3000.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -429,7 +429,6 @@ void * data) { assert (hdr); - assert (data); if (provideStereo) ProcessMessageCamera (resp_queue, hdr, data, stereo_addr); Modified: code/player/trunk/server/drivers/position/ascension/flockofbirds.cc =================================================================== --- code/player/trunk/server/drivers/position/ascension/flockofbirds.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/position/ascension/flockofbirds.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -492,9 +492,6 @@ int FlockOfBirds_Device::ProcessMessage(QueuePointer & resp_queue, player_msghdr * hdr, void * data) { - assert(hdr); - assert(data); - return -1; } Modified: code/player/trunk/server/drivers/position/bumpersafe/bumpersafe.cc =================================================================== --- code/player/trunk/server/drivers/position/bumpersafe/bumpersafe.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/position/bumpersafe/bumpersafe.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -161,7 +161,6 @@ void * data) { assert(hdr); - assert(data); if (hdr->type==PLAYER_MSGTYPE_SYNCH) { Modified: code/player/trunk/server/drivers/position/isense/inertiacube2.cc =================================================================== --- code/player/trunk/server/drivers/position/isense/inertiacube2.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/position/isense/inertiacube2.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -454,7 +454,6 @@ int InertiaCube2::ProcessMessage (QueuePointer &resp_queue, player_msghdr * hdr, void * data) { assert(hdr); - assert(data); if(Message::MatchMessage (hdr, PLAYER_MSGTYPE_DATA , PLAYER_POSITION2D_DATA_STATE, position_id)) { Modified: code/player/trunk/server/drivers/position/lasersafe/lasersafe.cc =================================================================== --- code/player/trunk/server/drivers/position/lasersafe/lasersafe.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/position/lasersafe/lasersafe.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -274,8 +274,6 @@ int LaserSafe::ProcessMessage (QueuePointer & resp_queue, player_msghdr * hdr, void * data) { assert(hdr); - assert(data); - if (hdr->type==PLAYER_MSGTYPE_SYNCH) { return 0; Modified: code/player/trunk/server/drivers/position/mbicp/mbicp_driver.cc =================================================================== --- code/player/trunk/server/drivers/position/mbicp/mbicp_driver.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/position/mbicp/mbicp_driver.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -7,7 +7,7 @@ J. Minguez, L. Montesano, and F. Lamiraux, "Metric-based iterative closest point scan matching for sensor displacement estimation," IEEE -Transactions on Robotics, vol. 22, no. 5, pp. 1047 \u2013 1054, 2006. +Transactions on Robotics, vol. 22, no. 5, pp. 1047 \u2013 1054, 2006. @par Compile-time dependencies @@ -20,7 +20,7 @@ @par Requires - @ref interface_position2d : source of pose and velocity information -- @ref interface_laser : Pose-stamped laser scans (subtype +- @ref interface_laser : Pose-stamped laser scans (subtype PLAYER_LASER_DATA_SCANPOSE) @par Configuration requests @@ -30,111 +30,111 @@ @par Configuration file options - max_laser_range (float) - - Default: 7.9 m + - Default: 7.9 m - Maximum laser range. - laserPose_x (float) - - Default: 0.16 m + - Default: 0.16 m - Offset of the laser on the edge x (in the robot's system of reference). - laserPose_y (float) - - Default: 0.0 m + - Default: 0.0 m - Offset of the laser on the edge y (in the robot's system of reference). - + - laserPose_th (float) - - Default: 0.0 rad + - Default: 0.0 rad - Offset of the laser on th (in the robot's system of reference). - radial_window (float) - - Default: 0.3 m - - Maximum distance difference between points of different scans. Points + - Default: 0.3 m + - Maximum distance difference between points of different scans. Points with greater Br cannot be correspondent (eliminate spurius asoc.). - + - angular_window (float) - Default: 0.523333333 rad - - Maximum angle diference between points of different scans. Points - with greater Bw cannot be correspondent (eliminate spurius asoc.). - + - Maximum angle diference between points of different scans. Points + with greater Bw cannot be correspondent (eliminate spurius asoc.). + - L (float) - - Default: 3.00 - - Value of the metric. When L tends to infinity you are using the - standart ICP. When L tends to 0 you use the metric (more importance + - Default: 3.00 + - Value of the metric. When L tends to infinity you are using the + standart ICP. When L tends to 0 you use the metric (more importance to rotation), when L tends to infinity you are using Euclidian metric. - laserStep (integer) - - Default: 1 + - Default: 1 - Selects points of each scan with an step laserStep. When laserStep=1 uses all the points of the scans - When laserStep=2 uses one each two ... - This is an speed up parameter. - + When laserStep=2 uses one each two ... + This is an speed up parameter. + - MaxDistInter (float) - - Default: 0.5 m - - Maximum distance to interpolate between points in the ref scan. Consecutive - points with less Euclidean distance than MaxDistInter are considered + - Default: 0.5 m + - Maximum distance to interpolate between points in the ref scan. Consecutive + points with less Euclidean distance than MaxDistInter are considered to be a segment. - filter (float) - - Default: 0.95 - - In [0,1] sets the % of asociations NOT considered spurious. E.g. if - filter=0.9 you use 90% of the associations. The associations - are ordered by distance and the (1-filter) with greater distance + - Default: 0.95 + - In [0,1] sets the % of asociations NOT considered spurious. E.g. if + filter=0.9 you use 90% of the associations. The associations + are ordered by distance and the (1-filter) with greater distance are not used. This type of filtering is called "trimmed-ICP". - + - ProjectionFilter (int) - - Default: 1 - - Eliminate the points that cannot be seen given the two scans - (see Lu&Millios 97). It works well for angles < 45 deg. + - Default: 1 + - Eliminate the points that cannot be seen given the two scans + (see Lu&Millios 97). It works well for angles < 45 deg. 1 : activates the filter. 0 : desactivates the filter. - AsocError (float) - - Default: 0.1 + - Default: 0.1 - In [0,1]. Sets the % of minimun associations to run the algorithm. - One way to check if the algorithm diverges is to supervise - if the number of associatios goes below a thresold. When the number + One way to check if the algorithm diverges is to supervise + if the number of associatios goes below a thresold. When the number of associations is below AsocError, the main function will return error in associations step. - + - MaxIter (int) - - Default: 50 - - Sets the maximum number of iterations for the algorithm to exit. The + - Default: 50 + - Sets the maximum number of iterations for the algorithm to exit. The more iterations, the more chance you give the algorithm to be more accurate. - errorRatio (float) - Default: 0.0001 m - - In [0,1] sets the maximum error ratio between iterations to exit. In - iteration K, let be errorK the residual of the minimization. - Error_th=(errorK-1/errorK). When error_th tends to 1 more precise is + - In [0,1] sets the maximum error ratio between iterations to exit. In + iteration K, let be errorK the residual of the minimization. + Error_th=(errorK-1/errorK). When error_th tends to 1 more precise is the solution of the scan matching. - IterSmoothConv (int) - - Default: 2 - - Number of consecutive iterations that satisfity the error criteria + - Default: 2 + - Number of consecutive iterations that satisfity the error criteria (the two above criteria) (error_th) OR (errorx_out && errory_out && errt_out). - With this parameter >1 avoids random solutions and estabilices the algorithm. + With this parameter >1 avoids random solutions and estabilices the algorithm. - errx_out (float) - - Default: 0.0001 m - - Minimum error in x of the asociations to exit. In each iteration, the error - is the residual of the minimization in each component. The condition is - (errorKx<errx_out && errorKx<erry_out && errorKx<errt_out). When errorK + - Default: 0.0001 m + - Minimum error in x of the asociations to exit. In each iteration, the error + is the residual of the minimization in each component. The condition is + (errorKx<errx_out && errorKx<erry_out && errorKx<errt_out). When errorK tends to 0 the more precise is the solution of the scan matching - + - erry_out (float) - - Default: 0.0001 m - - Minimum error in x of the asociations to exit. In each iteration, the error - is the residual of the minimization in each component. The condition is - (errorKx<errx_out && errorKx<erry_out && errorKx<errt_out). When errorK + - Default: 0.0001 m + - Minimum error in x of the asociations to exit. In each iteration, the error + is the residual of the minimization in each component. The condition is + (errorKx<errx_out && errorKx<erry_out && errorKx<errt_out). When errorK tends to 0 the more precise is the solution of the scan matching - errt_out (float) - - Default: 0.0001 m - - Minimum error in x of the asociations to exit. In each iteration, the error - is the residual of the minimization in each component. The condition is - (errorKx<errx_out && errorKx<erry_out && errorKx<errt_out). When errorK + - Default: 0.0001 m + - Minimum error in x of the asociations to exit. In each iteration, the error + is the residual of the minimization in each component. The condition is + (errorKx<errx_out && errorKx<erry_out && errorKx<errt_out). When errorK tends to 0 the more precise is the solution of the scan matching - + @par Example @verbatim @@ -148,23 +148,23 @@ laserPose_x 0.16 laserPose_y 0 laserPose_th 0 - - radial_window 0.3 - angular_window 0.523333333 - - L 3.00 - laserStep 1 - MaxDistInter 0.5 - filter 0.95 - ProjectionFilter 1 - AsocError 0.1 - MaxIter 50 - - errorRatio 0.0001 - errx_out 0.0001 - erry_out 0.0001 - errt_out 0.0001 - IterSmoothConv 2 + + radial_window 0.3 + angular_window 0.523333333 + + L 3.00 + laserStep 1 + MaxDistInter 0.5 + filter 0.95 + ProjectionFilter 1 + AsocError 0.1 + MaxIter 50 + + errorRatio 0.0001 + errx_out 0.0001 + erry_out 0.0001 + errt_out 0.0001 + IterSmoothConv 2 ) @endverbatim @@ -185,7 +185,7 @@ { public: - + mbicp( ConfigFile* cf, int section); virtual ~mbicp(); @@ -213,7 +213,7 @@ float errt_out; int IterSmoothConv; Tsc laserPoseTsc; - + player_pose2d_t lastPoseOdom, currentPose, previousPose, @@ -221,7 +221,7 @@ player_laser_data_t currentScan, previousScan; - + bool havePrevious; //Compute scanMatching @@ -300,21 +300,21 @@ //////////////////////////////////////////////////////////////////////////////// void mbicp::setupScanMatching(){ -Init_MbICP_ScanMatching( +Init_MbICP_ScanMatching( this->max_laser_range, - this->Bw, - this->Br, - this->L, + this->Bw, + this->Br, + this->L, this->laserStep, this->MaxDistInter, - this->filter, + this->filter, this->ProjectionFilter, this->AsocError, - this->MaxIter, + this->MaxIter, this->errorRatio, - this->errx_out, - this->erry_out, - this->errt_out, + this->errx_out, + this->erry_out, + this->errt_out, this->IterSmoothConv); } @@ -420,7 +420,7 @@ cmd.vel.px = 0; cmd.vel.py = 0; cmd.vel.pa = 0; - + odom->PutMsg(InQueue, PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL, (void*)&cmd,sizeof(cmd),NULL); @@ -436,7 +436,7 @@ while (true){ // Wait till we get new data Wait(); - + // Test if we are supposed to cancel this thread. pthread_testcancel(); @@ -452,7 +452,6 @@ // PLAYER_LASER_DATA_SCANPOSE if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,PLAYER_LASER_DATA_SCANPOSE, laser_addr)) { - assert(hdr->size == sizeof(player_laser_data_scanpose_t)); ProcessSubtypeLaser(hdr, *reinterpret_cast<player_laser_data_scanpose_t *> (data)); }else @@ -469,7 +468,7 @@ player_msghdr_t newhdr = *hdr; newhdr.addr = device_addr; Publish(&newhdr, (void*)&data); - }else + }else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,/*PLAYER_POSITION2D_CMD_VEL*/ -1, device_addr)) { @@ -479,7 +478,7 @@ newhdr.addr = odom_addr; odom->PutMsg(InQueue, &newhdr, (void*)data); }else - { + { if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, -1, device_addr)){ // Pass the request on to the underlying position device and wait for // the reply. @@ -505,14 +504,14 @@ void mbicp::ProcessOdom(player_msghdr_t* hdr,player_position2d_data_t &data) { - + Tsc outComposicion1, outComposicion2, outInversion1; - + Tsc lastPoseOdomTsc, previousPoseTsc, - scanmatchingPoseTsc; + scanmatchingPoseTsc; lastPoseOdom.px = data.pos.px; @@ -528,15 +527,15 @@ inversion_sis(&previousPoseTsc, &outInversion1); composicion_sis(&outInversion1, &lastPoseOdomTsc, &outComposicion1); composicion_sis(&scanmatchingPoseTsc, &outComposicion1, &outComposicion2); - + data.pos.px = outComposicion2.x; data.pos.py = outComposicion2.y; data.pos.pa = outComposicion2.tita; - } - + } + player_msghdr_t newhdr = *hdr; newhdr.addr = device_addr; Publish(&newhdr, (void*)&data); @@ -552,20 +551,20 @@ currentPose = lastPoseOdom; - currentScan.min_angle = data.scan.min_angle; - currentScan.max_angle = data.scan.max_angle; + currentScan.min_angle = data.scan.min_angle; + currentScan.max_angle = data.scan.max_angle; currentScan.resolution = data.scan.resolution; - currentScan.max_range = data.scan.max_range; + currentScan.max_range = data.scan.max_range; currentScan.ranges_count = data.scan.ranges_count; - currentScan.intensity_count = data.scan.intensity_count; + currentScan.intensity_count = data.scan.intensity_count; currentScan.id = data.scan.id; - + for (unsigned int i=0; i < currentScan.ranges_count; i++){ currentScan.ranges[i] = data.scan.ranges[i]; currentScan.intensity[i] = data.scan.intensity[i]; } - if (havePrevious && ( currentPose.px != previousPose.px || + if (havePrevious && ( currentPose.px != previousPose.px || currentPose.py != previousPose.py || currentPose.pa != previousPose.pa)) { @@ -578,7 +577,7 @@ previousPose = currentPose; scanmatchingPose = currentPose; havePrevious = true; - } + } } @@ -589,7 +588,7 @@ currentPoseTsc, scanmatchingPoseTsc, solutionTsc; - + Tsc outComposicion1, outComposicion2, outComposicion3, @@ -598,13 +597,13 @@ outComposicion11, outInversion1, outInversion4; - + Tpfp previousScanTpfp[LASER_MAX_SAMPLES], currentScanTpfp[LASER_MAX_SAMPLES]; - - int salidaMbicp; - + int salidaMbicp; + + currentPoseTsc = playerPose2Tsc(currentPose); previousPoseTsc = playerPose2Tsc(previousPose); scanmatchingPoseTsc = playerPose2Tsc(scanmatchingPose); @@ -618,7 +617,7 @@ playerLaser2Tpfp(previousScan,previousScanTpfp); playerLaser2Tpfp(currentScan,currentScanTpfp); - salidaMbicp = MbICPmatcher(previousScanTpfp,currentScanTpfp,&outComposicion3, &solutionTsc); + salidaMbicp = MbICPmatcher(previousScanTpfp,currentScanTpfp,&outComposicion3, &solutionTsc); if (salidaMbicp == 1){ @@ -635,7 +634,7 @@ else{ if (salidaMbicp == 2) - fprintf(stderr,"2 : Everything OK but reached the Maximum number of iterations\n"); + fprintf(stderr,"2 : Everything OK but reached the Maximum number of iterations\n"); else{ if (salidaMbicp == -1) fprintf(stderr,"Failure in the association step\n"); @@ -663,7 +662,7 @@ Tsc mbicp::playerPose2Tsc(player_pose2d_t posicion) { Tsc posicionTsc; - + posicionTsc.x = posicion.px; posicionTsc.y = posicion.py; posicionTsc.tita = posicion.pa; @@ -675,7 +674,7 @@ player_pose2d_t mbicp::Tsc2playerPose(Tsc posicion) { player_pose2d_t posicionPlayer; - + posicionPlayer.px = posicion.x; posicionPlayer.py = posicion.y; posicionPlayer.pa = posicion.tita; Modified: code/player/trunk/server/drivers/position/roboteq/roboteq.cc =================================================================== --- code/player/trunk/server/drivers/position/roboteq/roboteq.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/position/roboteq/roboteq.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -23,7 +23,7 @@ /** @{ */ /** @defgroup driver_roboteq roboteq * @brief Motor control driver for Roboteq AX2550 - + Provides position2d interface to the Roboteq AX2550 motor controller http://www.roboteq.com/ax2550-folder.html This driver ignores all configuration requests and produces no data @@ -77,7 +77,7 @@ driver ( name "roboteq" - provides ["position2d:0"] + provides ["position2d:0"] devicepath "/dev/ttyS0" max_trans_spd 6.0 max_rot_spd 4.0 @@ -95,15 +95,15 @@ #include <termios.h> #include <sys/ioctl.h> // ioctl #include <unistd.h> // close(2),fcntl(2),getpid(2),usleep(3),execvp(3),fork(2) -#include <netdb.h> // for gethostbyname(3) -#include <netinet/in.h> // for struct sockaddr_in, htons(3) -#include <sys/types.h> // for socket(2) -#include <sys/socket.h> // for socket(2) -#include <signal.h> // for kill(2) -#include <fcntl.h> // for fcntl(2) -#include <string.h> // for strncpy(3),memcpy(3) -#include <stdlib.h> // for atexit(3),atoi(3) -#include <pthread.h> // for pthread stuff +#include <netdb.h> // for gethostbyname(3) +#include <netinet/in.h> // for struct sockaddr_in, htons(3) +#include <sys/types.h> // for socket(2) +#include <sys/socket.h> // for socket(2) +#include <signal.h> // for kill(2) +#include <fcntl.h> // for fcntl(2) +#include <string.h> // for strncpy(3),memcpy(3) +#include <stdlib.h> // for atexit(3),atoi(3) +#include <pthread.h> // for pthread stuff #include <math.h> #include <libplayercore/playercore.h> @@ -112,7 +112,7 @@ #define SERIAL_BUFF_SIZE 128 #define MAX_MOTOR_SPEED 127 #define ROBOTEQ_CON_TIMEOUT 10 // seconds to time-out on setting RS-232 mode -#define ROBOTEQ_DEFAULT_BAUD 9600 +#define ROBOTEQ_DEFAULT_BAUD 9600 #ifndef CRTSCTS #ifdef IHFLOW @@ -165,8 +165,8 @@ public: roboteq( ConfigFile* cf, int section); - - virtual int ProcessMessage(QueuePointer &resp_queue, + + virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr * hdr, void * data); virtual int Setup(); virtual int Shutdown(); @@ -191,7 +191,7 @@ /////////////////////////////////////////////////////////////////////////// roboteq::roboteq( ConfigFile* cf, int section) : Driver(cf, section) -{ +{ memset (&this->position2d_id, 0, sizeof (player_devaddr_t)); // Outgoing position 2d interface @@ -199,12 +199,12 @@ PLAYER_POSITION2D_CODE, -1, NULL) == 0){ if(this->AddInterface(this->position2d_id) != 0){ this->SetError(-1); - return; + return; } } double max_trans_spd, max_rot_spd; - // required parameter(s) + // required parameter(s) if(!(this->devicepath = (char*)cf->ReadString(section, "devicepath", NULL))){ PLAYER_ERROR("must specify devicepath"); this->SetError(-1); @@ -244,10 +244,10 @@ roboteq_fd = open(devicepath, O_RDWR|O_NDELAY); if (roboteq_fd == -1){ fputs("Unable to configure serial port for RoboteQ!", stderr); - return 0; + return 0; }else{ struct termios options; - + tcgetattr(roboteq_fd, &options); // default is 9600 unless otherwise specified @@ -287,7 +287,7 @@ // initialize RoboteQ to RS-232 mode strcpy(serialout_buff, "\r"); - for (i=0; i<10; i++){ + for (i=0; i<10; i++){ write(roboteq_fd, serialout_buff, 1); tcdrain(roboteq_fd); usleep(25000); @@ -311,9 +311,9 @@ else fputs("Successfully initialized Roboteq connection.\n", stderr); - fputs("Done.\n", stderr); + fputs("Done.\n", stderr); - // now spawn reading thread + // now spawn reading thread StartThread(); return(0); @@ -346,11 +346,11 @@ // no 'W's for ROBOTEQ_CON_TIMEOUT seconds // means we're probably in RC mode again - // 07-09-07 + // 07-09-07 // this test may need to change since the reset // appears to fail quite often. is it really // failing or is the test bad? - return 0; + return 0; } memset(serialin_buff, 0, SERIAL_BUFF_SIZE); ret = read(roboteq_fd, serialin_buff, SERIAL_BUFF_SIZE); @@ -358,7 +358,7 @@ fputs("Unable to reset Roboteq to RC mode!", stderr); close(roboteq_fd); - + return(0); } @@ -368,22 +368,21 @@ int roboteq::ProcessMessage (QueuePointer &resp_queue, player_msghdr * hdr, void * data) { assert(hdr); - assert(data); /* - fprintf(stderr, "ProcessMessage: type=%d subtype=%d\n", + fprintf(stderr, "ProcessMessage: type=%d subtype=%d\n", hdr->type, hdr->subtype); - */ - if (Message::MatchMessage(hdr, PLAYER_POSITION2D_REQ_MOTOR_POWER, - PLAYER_POSITION2D_CMD_VEL, position2d_id)){ + */ + if (Message::MatchMessage(hdr, PLAYER_POSITION2D_REQ_MOTOR_POWER, + PLAYER_POSITION2D_CMD_VEL, position2d_id)){ assert(hdr->size == sizeof(player_position2d_cmd_vel_t)); - player_position2d_cmd_vel_t & command + player_position2d_cmd_vel_t & command = *reinterpret_cast<player_position2d_cmd_vel_t *> (data); - + // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - // convert from the generic position interface + // convert from the generic position interface // to the Roboteq-specific command - // assumes "Mixed Mode" -- + // assumes "Mixed Mode" -- // channel 1 : FW/BW speed // channel 2 : rotation // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -392,7 +391,7 @@ float vel_yawspd = command.vel.pa; //fprintf(stderr, "ProcessMessage: trans=%f, steer=%f\n", vel_trans, vel_turret); - + // scale and translate to Roboteq command vel_xtrans = (double)vel_xtrans * speed_scaling_factor; vel_yawspd = (double)vel_yawspd * rot_scaling_factor; @@ -409,7 +408,7 @@ */ return 0; } - + return -1; } @@ -419,20 +418,20 @@ void roboteq::Main() { double position_time=0; - + pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED,NULL); for(;;){ ProcessMessages(); pthread_testcancel(); // publish dummy data - Publish(device_addr, PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE, + Publish(device_addr, PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE, (unsigned char*) &data, sizeof(data), &position_time); usleep(10); } pthread_exit(NULL); - + return; } @@ -447,22 +446,22 @@ else if (trans_command < -MAX_MOTOR_SPEED) trans_command = -MAX_MOTOR_SPEED; if (rot_command > MAX_MOTOR_SPEED) rot_command = MAX_MOTOR_SPEED; else if (rot_command < -MAX_MOTOR_SPEED) rot_command = -MAX_MOTOR_SPEED; - + if (trans_command > 0) strcpy(speed, FORWARD); else strcpy(speed, REVERSE); if (rot_command > 0) strcpy(heading, LEFT); else strcpy(heading, RIGHT); - + // form motor cmd string strcpy(cmd_str, speed); snprintf(cmd_str+2, 4, "%.2x", abs(trans_command)); // start at char 3 - strcat(cmd_str, "\r"); + strcat(cmd_str, "\r"); strcat(cmd_str, heading); snprintf(cmd_str + strlen(cmd_str), 4, "%.2x", abs(rot_command)); strcat(cmd_str, "\r"); - + return 0; } Modified: code/player/trunk/server/drivers/ptz/amtecpowercube.cc =================================================================== --- code/player/trunk/server/drivers/ptz/amtecpowercube.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/ptz/amtecpowercube.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -999,7 +999,6 @@ int AmtecPowerCube::ProcessMessage(QueuePointer &resp_queue, player_msghdr * hdr, void * data) { assert(hdr); - assert(data); if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_PTZ_CMD_STATE, device_addr)) { Modified: code/player/trunk/server/drivers/ptz/canonvcc4.cc =================================================================== --- code/player/trunk/server/drivers/ptz/canonvcc4.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/ptz/canonvcc4.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -949,7 +949,6 @@ int canonvcc4::ProcessMessage(QueuePointer &resp_queue, player_msghdr * hdr, void * data) { assert(hdr); - assert(data); if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, Modified: code/player/trunk/server/drivers/ptz/ptu46.cc =================================================================== --- code/player/trunk/server/drivers/ptz/ptu46.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/ptz/ptu46.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -632,7 +632,6 @@ PTU46_Device::ProcessMessage (QueuePointer &resp_queue, player_msghdr * hdr, void * data) { assert (hdr); - assert (data); // No REQ_GENERIC if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, PLAYER_PTZ_REQ_GENERIC, device_addr)) Modified: code/player/trunk/server/drivers/ptz/sonyevid30.cc =================================================================== --- code/player/trunk/server/drivers/ptz/sonyevid30.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/ptz/sonyevid30.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -1377,7 +1377,6 @@ player_msghdr * hdr, void *data) { assert(hdr); - assert(data); if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_PTZ_REQ_GENERIC, device_addr)) { Modified: code/player/trunk/server/drivers/rfid/sickrfi341.cc =================================================================== --- code/player/trunk/server/drivers/rfid/sickrfi341.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/rfid/sickrfi341.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -203,9 +203,6 @@ player_msghdr* hdr, void* data) { - assert (hdr); - assert (data); - return (-1); } Modified: code/player/trunk/server/drivers/speech/festival.cc =================================================================== --- code/player/trunk/server/drivers/speech/festival.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/speech/festival.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -402,9 +402,6 @@ int Festival::ProcessMessage(QueuePointer & resp_queue, player_msghdr * hdr, void * data) { - assert(hdr); - assert(data); - if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_SPEECH_CMD_SAY, device_addr)) { player_speech_cmd_t * cmd = (player_speech_cmd_t *) data; Modified: code/player/trunk/server/drivers/vectormap/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/vectormap/CMakeLists.txt 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/vectormap/CMakeLists.txt 2008-12-18 19:58:30 UTC (rev 7206) @@ -9,4 +9,4 @@ PLAYERDRIVER_OPTION (vec2map build_vec2map ON) PLAYERDRIVER_REQUIRE_HEADER (vec2map build_vec2map geos_c.h) -PLAYERDRIVER_ADD_DRIVER (postgis build_postgis LINKFLAGS "-lgeos" SOURCES vec2map.cc) +PLAYERDRIVER_ADD_DRIVER (vec2map build_vec2map LINKFLAGS "-lgeos" SOURCES vec2map.cc) Modified: code/player/trunk/server/drivers/vectormap/vec2map.cc =================================================================== --- code/player/trunk/server/drivers/vectormap/vec2map.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/vectormap/vec2map.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -1,6 +1,6 @@ /* * Player - One Hell of a Robot Server - * Copyright (C) 2004 Brian Gerkey ge...@st... + * 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 @@ -57,7 +57,7 @@ - Default: "" - Do not draw feature with that name on final grid map -@par Example +@par Example @verbatim driver @@ -103,7 +103,7 @@ class Vec2Map : public Driver { - public: + public: // Constructor; need that Vec2Map(ConfigFile * cf, int section); @@ -114,7 +114,7 @@ virtual int Shutdown(); // This method will be invoked on each incoming message - virtual int ProcessMessage(QueuePointer & resp_queue, + virtual int ProcessMessage(QueuePointer & resp_queue, player_msghdr * hdr, void * data); @@ -223,7 +223,7 @@ //////////////////////////////////////////////////////////////////////////////// // Set up the device. Return 0 if things go well, and -1 otherwise. int Vec2Map::Setup() -{ +{ // Retrieve the handle to the vectormap device. this->vectormap_dev = deviceTable->GetDevice(this->vectormap_addr); if (!(this->vectormap_dev)) @@ -251,7 +251,7 @@ { // Stop and join the driver thread StopThread(); - + // Unsubscribe from the vectormap this->vectormap_dev->Unsubscribe(this->InQueue); @@ -260,7 +260,7 @@ //////////////////////////////////////////////////////////////////////////////// // Main function for device thread -void Vec2Map::Main() +void Vec2Map::Main() { struct timespec tspec; @@ -271,7 +271,7 @@ this->InQueue->Wait(); pthread_testcancel(); - + // Process incoming messages ProcessMessages(); @@ -411,10 +411,10 @@ if (over(static_cast<int>(x), 0, width)) break; if (over(static_cast<int>(y), 0, height)) break; cells[(static_cast<int>(y) * width) + (static_cast<int>(x))] = 1; - } + } } -int Vec2Map::ProcessMessage(QueuePointer & resp_queue, +int Vec2Map::ProcessMessage(QueuePointer & resp_queue, player_msghdr * hdr, void * data) { @@ -768,7 +768,7 @@ // 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 Vec2Map_Register(DriverTable * table) +void vec2map_Register(DriverTable * table) { table->AddDriver("vec2map", Vec2Map_Init); } Modified: code/player/trunk/server/drivers/wifi/linuxwifi.cc =================================================================== --- code/player/trunk/server/drivers/wifi/linuxwifi.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/wifi/linuxwifi.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -274,9 +274,6 @@ int LinuxWiFi::ProcessMessage(QueuePointer & resp_queue, player_msghdr * hdr, void * data) { - assert(hdr); - assert(data); - if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_WIFI_REQ_MAC, device_addr)) { player_wifi_mac_req_t req; Modified: code/player/trunk/server/drivers/wsn/accel_calib.cc =================================================================== --- code/player/trunk/server/drivers/wsn/accel_calib.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/wsn/accel_calib.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -297,9 +297,6 @@ player_wsn_data_t new_wsn_data; player_wsn_data_t* original_wsn_data; - assert (hdr); - assert (data); - // Handle new data from the WSN device if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_DATA, PLAYER_WSN_DATA_STATE, wsn_addr)) Modified: code/player/trunk/server/drivers/wsn/mica2.cc =================================================================== --- code/player/trunk/server/drivers/wsn/mica2.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/wsn/mica2.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -474,9 +474,6 @@ player_msghdr * hdr, void * data) { - assert (hdr); - assert (data); - if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_CMD, PLAYER_WSN_CMD_DEVSTATE, wsn_addr)) { Modified: code/player/trunk/server/drivers/wsn/rcore_xbridge.cc =================================================================== --- code/player/trunk/server/drivers/wsn/rcore_xbridge.cc 2008-12-18 19:43:37 UTC (rev 7205) +++ code/player/trunk/server/drivers/wsn/rcore_xbridge.cc 2008-12-18 19:58:30 UTC (rev 7206) @@ -309,9 +309,6 @@ player_msghdr * hdr, void * data) { - assert (hdr); - assert (data); - if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, PLAYER_WSN_REQ_POWER, device_addr)) { This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <gb...@us...> - 2008-12-19 02:11:14
|
Revision: 7208 http://playerstage.svn.sourceforge.net/playerstage/?rev=7208&view=rev Author: gbiggs Date: 2008-12-19 02:10:56 +0000 (Fri, 19 Dec 2008) Log Message: ----------- Applied patch #2152616, fixed a double-free in hokuyo_aist driver Modified Paths: -------------- code/player/trunk/examples/plugins/exampledriver/exampledriver.cc code/player/trunk/examples/plugins/multidriver/multidriver.cc code/player/trunk/examples/plugins/opaquedriver/opaquedriver.cc code/player/trunk/libplayercore/CMakeLists.txt code/player/trunk/libplayercore/device.cc code/player/trunk/libplayercore/device.h code/player/trunk/libplayercore/devicetable.cc code/player/trunk/libplayercore/driver.cc code/player/trunk/libplayercore/driver.h code/player/trunk/server/drivers/actarray/amtecM5.cc code/player/trunk/server/drivers/audio/alsa.cc code/player/trunk/server/drivers/audio/alsa.h code/player/trunk/server/drivers/base/imagebase.cc code/player/trunk/server/drivers/base/imagebase.h code/player/trunk/server/drivers/blackboard/localbb/localbb.cpp code/player/trunk/server/drivers/blobfinder/acts/acts.cc code/player/trunk/server/drivers/blobfinder/cmvision/P2CMV.cc code/player/trunk/server/drivers/camera/1394/camera1394.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 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/uvc/cameraUVC.cc code/player/trunk/server/drivers/camera/uvc/cameraUVC.h code/player/trunk/server/drivers/camera/v4l/camerav4l.cc code/player/trunk/server/drivers/camera/yarp/YarpImage.cc code/player/trunk/server/drivers/fiducial/laserbar.cc code/player/trunk/server/drivers/gps/garminnmea.cc code/player/trunk/server/drivers/health/statgrab/statgrab_health.cpp code/player/trunk/server/drivers/health/statgrab/statgrab_health.h code/player/trunk/server/drivers/imu/XSensMT.cc code/player/trunk/server/drivers/imu/nimuplayer.cpp 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/pbs_driver.cc code/player/trunk/server/drivers/laser/sicklms200.cc code/player/trunk/server/drivers/laser/sicklms400.cc code/player/trunk/server/drivers/laser/sicks3000.cc code/player/trunk/server/drivers/laser/urglaserdriver.cc code/player/trunk/server/drivers/limb/eeDHcontroller.cc code/player/trunk/server/drivers/localization/amcl/amcl.cc code/player/trunk/server/drivers/localization/amcl/amcl.h code/player/trunk/server/drivers/localization/fakelocalize.cc code/player/trunk/server/drivers/mcom/lifomcom.cc code/player/trunk/server/drivers/mixed/botrics/obot.cc code/player/trunk/server/drivers/mixed/chatterbox/cb_driver.cc code/player/trunk/server/drivers/mixed/clodbuster/clodbuster.cc code/player/trunk/server/drivers/mixed/clodbuster/clodbuster.h 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/khepera/khepera.cc code/player/trunk/server/drivers/mixed/khepera/khepera.h code/player/trunk/server/drivers/mixed/mricp/src/mricp_driver.cpp code/player/trunk/server/drivers/mixed/nomad/nomad.cc 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/bumpersafe/bumpersafe.cc code/player/trunk/server/drivers/position/isense/inertiacube2.cc code/player/trunk/server/drivers/position/lasersafe/lasersafe.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/motionmind/motionmind.cc code/player/trunk/server/drivers/position/nav200/sicknav200.cc code/player/trunk/server/drivers/position/nd/nd_plugin.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/amtecpowercube.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/ranger/hokuyo_aist.cc code/player/trunk/server/drivers/rfid/acr120u.cc code/player/trunk/server/drivers/rfid/insideM300.cc code/player/trunk/server/drivers/rfid/phidgetRFID.cc code/player/trunk/server/drivers/rfid/sickrfi341.cc code/player/trunk/server/drivers/rfid/skyetekM1.cc code/player/trunk/server/drivers/service_adv/mdns.cc code/player/trunk/server/drivers/shell/dummy.cc code/player/trunk/server/drivers/shell/kartowriter.cc code/player/trunk/server/drivers/shell/passthrough.cc code/player/trunk/server/drivers/shell/readlog.cc code/player/trunk/server/drivers/shell/writelog.cc code/player/trunk/server/drivers/sonar/aiotosonar.cc code/player/trunk/server/drivers/speech/festival.cc code/player/trunk/server/drivers/speech/recognition/sphinx2.cc code/player/trunk/server/drivers/vectormap/vec2map.cc code/player/trunk/server/drivers/wifi/iwspy.cc code/player/trunk/server/drivers/wsn/accel_calib.cc code/player/trunk/server/drivers/wsn/mica2.cc code/player/trunk/server/drivers/wsn/phidgetAcc.cc code/player/trunk/server/drivers/wsn/rcore_xbridge.cc Modified: code/player/trunk/examples/plugins/exampledriver/exampledriver.cc =================================================================== --- code/player/trunk/examples/plugins/exampledriver/exampledriver.cc 2008-12-19 00:59:47 UTC (rev 7207) +++ code/player/trunk/examples/plugins/exampledriver/exampledriver.cc 2008-12-19 02:10:56 UTC (rev 7208) @@ -38,17 +38,13 @@ //////////////////////////////////////////////////////////////////////////////// // The class for the driver -class ExampleDriver : public Driver +class ExampleDriver : public ThreadedDriver { public: // Constructor; need that ExampleDriver(ConfigFile* cf, int section); - // Must implement the following methods. - virtual int Setup(); - virtual int Shutdown(); - // This method will be invoked on each incoming message virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr * hdr, @@ -58,6 +54,8 @@ // Main function for device thread. virtual void Main(); + virtual int MainSetup(); + virtual void MainQuit(); int foop; }; @@ -86,7 +84,7 @@ // Constructor. Retrieve options from the configuration file and do any // pre-Setup() setup. ExampleDriver::ExampleDriver(ConfigFile* cf, int section) - : Driver(cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, + : ThreadedDriver(cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_POSITION2D_CODE) { // Read an option from the configuration file @@ -97,7 +95,7 @@ //////////////////////////////////////////////////////////////////////////////// // Set up the device. Return 0 if things go well, and -1 otherwise. -int ExampleDriver::Setup() +int ExampleDriver::MainSetup() { puts("Example driver initialising"); @@ -108,29 +106,20 @@ puts("Example driver ready"); - // Start the device thread; spawns a new thread and executes - // ExampleDriver::Main(), which contains the main loop for the driver. - StartThread(); - return(0); } //////////////////////////////////////////////////////////////////////////////// // Shutdown the device -int ExampleDriver::Shutdown() +void ExampleDriver::MainQuit() { puts("Shutting example driver down"); - // Stop and join the driver thread - StopThread(); - // Here you would shut the device down by, for example, closing a // serial port. puts("Example driver has been shutdown"); - - return(0); } int ExampleDriver::ProcessMessage(QueuePointer & resp_queue, Modified: code/player/trunk/examples/plugins/multidriver/multidriver.cc =================================================================== --- code/player/trunk/examples/plugins/multidriver/multidriver.cc 2008-12-19 00:59:47 UTC (rev 7207) +++ code/player/trunk/examples/plugins/multidriver/multidriver.cc 2008-12-19 02:10:56 UTC (rev 7208) @@ -44,16 +44,13 @@ //////////////////////////////////////////////////////////////////////////////// // The class for the driver -class MultiDriver : public Driver +class MultiDriver : public ThreadedDriver { public: // Constructor; need that MultiDriver(ConfigFile* cf, int section); - // Must implement the following methods. - virtual int Setup(); - virtual int Shutdown(); virtual int ProcessMessage(QueuePointer & resp_queue, player_msghdr * hdr, void * data); @@ -61,6 +58,8 @@ private: // Main function for device thread. virtual void Main(); + virtual int MainSetup(); + virtual void MainQuit(); // My position interface player_devaddr_t m_position_addr; @@ -111,7 +110,7 @@ // Constructor. Retrieve options from the configuration file and do any // pre-Setup() setup. MultiDriver::MultiDriver(ConfigFile* cf, int section) - : Driver(cf, section) + : ThreadedDriver(cf, section) { // Create my position interface if (cf->ReadDeviceAddr(&(this->m_position_addr), section, @@ -150,7 +149,7 @@ //////////////////////////////////////////////////////////////////////////////// // Set up the device. Return 0 if things go well, and -1 otherwise. -int MultiDriver::Setup() +int MultiDriver::MainSetup() { puts("Example driver initialising"); @@ -170,33 +169,21 @@ // configure a serial port. puts("Example driver ready"); - - // Start the device thread; spawns a new thread and executes - // MultiDriver::Main(), which contains the main loop for the driver. - this->StartThread(); - return(0); } //////////////////////////////////////////////////////////////////////////////// // Shutdown the device -int MultiDriver::Shutdown() +void MultiDriver::MainQuit() { puts("Shutting example driver down"); - // Stop and join the driver thread - this->StopThread(); - // Unsubscribe from the laser this->laser_dev->Unsubscribe(this->InQueue); // Here you would shut the device down by, for example, closing a // serial port. - - puts("Example driver has been shutdown"); - - return(0); } Modified: code/player/trunk/examples/plugins/opaquedriver/opaquedriver.cc =================================================================== --- code/player/trunk/examples/plugins/opaquedriver/opaquedriver.cc 2008-12-19 00:59:47 UTC (rev 7207) +++ code/player/trunk/examples/plugins/opaquedriver/opaquedriver.cc 2008-12-19 02:10:56 UTC (rev 7208) @@ -41,17 +41,13 @@ //////////////////////////////////////////////////////////////////////////////// // The class for the driver -class OpaqueDriver : public Driver +class OpaqueDriver : public ThreadedDriver { public: // Constructor; need that OpaqueDriver(ConfigFile* cf, int section); - // Must implement the following methods. - virtual int Setup(); - virtual int Shutdown(); - // This method will be invoked on each incoming message virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr * hdr, @@ -61,6 +57,8 @@ // Main function for device thread. virtual void Main(); + virtual int MainSetup(); + virtual void MainQuit(); // Update the data virtual void RefreshData(); @@ -96,7 +94,7 @@ // Constructor. Retrieve options from the configuration file and do any // pre-Setup() setup. OpaqueDriver::OpaqueDriver(ConfigFile* cf, int section) - : Driver(cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, + : ThreadedDriver(cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_OPAQUE_CODE) { mData.data_count = sizeof(test_t); @@ -115,7 +113,7 @@ //////////////////////////////////////////////////////////////////////////////// // Set up the device. Return 0 if things go well, and -1 otherwise. -int OpaqueDriver::Setup() +int OpaqueDriver::MainSetup() { puts("Example driver initialising"); @@ -125,29 +123,20 @@ puts("Opaque driver ready"); - // Start the device thread; spawns a new thread and executes - // OpaqueDriver::Main(), which contains the main loop for the driver. - StartThread(); - return(0); } //////////////////////////////////////////////////////////////////////////////// // Shutdown the device -int OpaqueDriver::Shutdown() +void OpaqueDriver::MainQuit() { puts("Shutting opaque driver down"); - // Stop and join the driver thread - StopThread(); - // Here you would shut the device down by, for example, closing a // serial port. puts("Opaque driver has been shutdown"); - - return(0); } int OpaqueDriver::ProcessMessage(QueuePointer & resp_queue, @@ -169,18 +158,13 @@ // The main loop; interact with the device here for(;;) { - // test if we are supposed to cancel - pthread_testcancel(); - + Wait(1); // Process incoming messages. OpaqueDriver::ProcessMessage() is // called on each message. ProcessMessages(); // Interact with the device, and push out the resulting data, using RefreshData(); - - // Sleep (you might, for example, block on a read() instead) - usleep(100000); } } Modified: code/player/trunk/libplayercore/CMakeLists.txt =================================================================== --- code/player/trunk/libplayercore/CMakeLists.txt 2008-12-19 00:59:47 UTC (rev 7207) +++ code/player/trunk/libplayercore/CMakeLists.txt 2008-12-19 02:10:56 UTC (rev 7208) @@ -41,7 +41,8 @@ wallclocktime.cc plugins.cc globals.cc - property.cpp) + property.cpp + threaded_driver.cc) # TODO: playerxdr should NOT be linked here; it's a bogus dependency coming # from the fact that message cloning functions are auto-generated into Modified: code/player/trunk/libplayercore/device.cc =================================================================== --- code/player/trunk/libplayercore/device.cc 2008-12-19 00:59:47 UTC (rev 7207) +++ code/player/trunk/libplayercore/device.cc 2008-12-19 02:10:56 UTC (rev 7208) @@ -63,13 +63,12 @@ #include <libplayercore/interface_util.h> // Constructor -Device::Device(player_devaddr_t addr, Driver *device) +Device::Device(player_devaddr_t addr, Driver *device) : + next(NULL), + addr(addr), + driver(device) { - this->next = NULL; - - this->addr = addr; - this->driver = device; - + pthread_mutex_init(&accessMutex,NULL); memset(this->drivername, 0, sizeof(this->drivername)); if(this->driver) @@ -101,6 +100,7 @@ delete this->driver; } free(this->queues); + pthread_mutex_destroy(&accessMutex); } int @@ -108,10 +108,9 @@ { int retval; size_t i; + + Lock(); - if(this->driver) - this->driver->Lock(); - // find an empty spot to put the new queue for(i=0;i<this->len_queues;i++) { @@ -142,19 +141,18 @@ { // remove the subscriber's queue, since the subscription failed this->queues[i] = QueuePointer(); - this->driver->Unlock(); + Unlock(); return(retval); } else if(retval == 1 && (retval = this->driver->Subscribe(this->addr))) { // remove the subscriber's queue, since the subscription failed this->queues[i] = QueuePointer(); - this->driver->Unlock(); + Unlock(); return(retval); } - - this->driver->Unlock(); } + Unlock(); return(0); } @@ -163,39 +161,32 @@ Device::Unsubscribe(QueuePointer &sub_queue) { int retval; - if(this->driver) { - this->driver->Lock(); - // first try the new version which passes the queue in retval = this->driver->Unsubscribe(sub_queue, this->addr); if (retval < 0) { // remove the subscriber's queue, since the subscription failed - this->driver->Unlock(); return(retval); } else if(retval == 1 && (retval = this->driver->Unsubscribe(this->addr))) { - this->driver->Unlock(); return(retval); } } + Lock(); // look for the given queue for(size_t i=0;i<this->len_queues;i++) { if(this->queues[i] == sub_queue) { this->queues[i] = QueuePointer(); - if(this->driver) - this->driver->Unlock(); + Unlock(); return(0); } } - //PLAYER_ERROR("tried to unsubscribed not-subscribed queue"); - if(this->driver) - this->driver->Unlock(); + Unlock(); return(-1); } @@ -258,9 +249,6 @@ double* timestamp, bool threaded) { - // check driver still ahs subscriptions, stops deadlocks on server shutdown - if (driver->subscriptions == 0) - return NULL; // Send the request message this->PutMsg(resp_queue, type, subtype, @@ -288,12 +276,14 @@ Message* msg = NULL; if(threaded) { - // pthread_cond_wait does not garuntee no false wake up, so maybe it is. // test driver is still subscribed to prevent deadlocks on server shutdown - while(driver->subscriptions > 0 && !(msg = resp_queue->Pop())) + while(driver->HasSubscriptions()) { - //PLAYER_WARN("empty queue after waiting!"); - resp_queue->Wait(); // this is a cancelation point + resp_queue->Wait(1); + msg = resp_queue->Pop(); + // a message is not the only reason a thread can be woken up, so continue to loop if we didn't get a message + if (msg) + break; } } else @@ -308,10 +298,9 @@ { Driver* dri = dev->driver; // don't update the requester - //if(dri->InQueue == resp_queue) if(!dri || dev->InQueue == resp_queue) continue; - if(!dri->driverthread && ((dri->subscriptions > 0) || dri->alwayson)) + if(((dri->HasSubscriptions()) || dri->alwayson)) dri->Update(); } @@ -320,7 +309,8 @@ } } - assert(msg); + if (!msg) + return msg; player_msghdr_t* hdr; hdr = msg->GetHeader(); @@ -352,3 +342,12 @@ } } +void Device::Lock() +{ + pthread_mutex_lock(&accessMutex); +} + +void Device::Unlock() +{ + pthread_mutex_unlock(&accessMutex); +} Modified: code/player/trunk/libplayercore/device.h =================================================================== --- code/player/trunk/libplayercore/device.h 2008-12-19 00:59:47 UTC (rev 7207) +++ code/player/trunk/libplayercore/device.h 2008-12-19 02:10:56 UTC (rev 7208) @@ -61,9 +61,7 @@ /// and hence appear more than once in the device table. class Device { - private: public: - /// @brief Constructor /// /// @param addr : Device address @@ -177,8 +175,6 @@ /// The string name for the underlying driver char drivername[PLAYER_MAX_DRIVER_STRING_LEN]; - /// Pointer to the underlying driver - Driver* driver; /// Pointer to the underlying driver's queue QueuePointer InQueue; @@ -188,6 +184,20 @@ /// Length of @p queues size_t len_queues; + + /// Pointer to the underlying driver + Driver* driver; + + private: + /** @brief Mutex used to lock access, via Lock() and Unlock(), to + device internals, like the list of subscribed queues. */ + pthread_mutex_t accessMutex; + /** @brief Lock access to driver internals. */ + void Lock(void); + /** @brief Unlock access to driver internals. */ + void Unlock(void); + + }; #endif Modified: code/player/trunk/libplayercore/devicetable.cc =================================================================== --- code/player/trunk/libplayercore/devicetable.cc 2008-12-19 00:59:47 UTC (rev 7207) +++ code/player/trunk/libplayercore/devicetable.cc 2008-12-19 02:10:56 UTC (rev 7208) @@ -65,20 +65,10 @@ { Device* thisentry; Device* tmpentry; - // First, shutdown each active driver - thisentry=head; - while(thisentry) - { - if(thisentry->driver->subscriptions) - { - thisentry->driver->Shutdown(); - thisentry->driver->subscriptions = 0; - thisentry->driver->alwayson = 0; - // wake up anything waiting on the drivers queue, so they can notice it is dead - thisentry->driver->InQueue->DataAvailable(); - } - thisentry = thisentry->next; - } + // First, Terminate each active driver + for (thisentry=head;thisentry;thisentry = thisentry->next) + thisentry->driver->Terminate(); + pthread_mutex_lock(&mutex); // Second, delete each device thisentry=head; @@ -254,7 +244,7 @@ for(thisentry=head;thisentry;thisentry=thisentry->next) { dri = thisentry->driver; - if((dri->subscriptions > 0) || dri->alwayson) + if((dri->HasSubscriptions()) || dri->alwayson) dri->Update(); } } Modified: code/player/trunk/libplayercore/driver.cc =================================================================== --- code/player/trunk/libplayercore/driver.cc 2008-12-19 00:59:47 UTC (rev 7207) +++ code/player/trunk/libplayercore/driver.cc 2008-12-19 02:10:56 UTC (rev 7208) @@ -72,7 +72,6 @@ int interf) : InQueue(overwrite_cmds, queue_maxlen) { this->error = 0; - this->driverthread = 0; // Look for our default device id if(cf->ReadDeviceAddr(&this->device_addr, section, "provides", @@ -95,6 +94,7 @@ } pthread_mutex_init(&this->accessMutex,NULL); + pthread_mutex_init(&this->subscriptionMutex,NULL); } // this is the other constructor, used by multi-interface drivers. @@ -102,7 +102,6 @@ bool overwrite_cmds, size_t queue_maxlen) : InQueue(overwrite_cmds, queue_maxlen) { this->error = 0; - this->driverthread = 0; this->device_addr.interf = 0xFFFF; @@ -111,6 +110,7 @@ this->entries = 0; pthread_mutex_init(&this->accessMutex,NULL); + pthread_mutex_init(&this->subscriptionMutex,NULL); } // destructor, to free up allocated queue. @@ -132,7 +132,7 @@ } int -Driver::AddInterface(player_devaddr_t *addr, ConfigFile * cf, int section, int code, char * key) +Driver::AddInterface(player_devaddr_t *addr, ConfigFile * cf, int section, int code, const char * key) { assert(addr); // Create position interface @@ -264,10 +264,25 @@ pthread_mutex_unlock(&accessMutex); } +void Driver::SubscriptionLock() +{ + pthread_mutex_lock(&subscriptionMutex); +} + +void Driver::SubscriptionUnlock() +{ + pthread_mutex_unlock(&subscriptionMutex); +} + +bool Driver::HasSubscriptions() +{ + return subscriptions > 0; +} + int Driver::Subscribe(player_devaddr_t addr) { int setupResult; - + SubscriptionLock(); if(subscriptions == 0) { setupResult = Setup(); @@ -279,14 +294,14 @@ subscriptions++; setupResult = 0; } - + SubscriptionUnlock(); return(setupResult); } int Driver::Unsubscribe(player_devaddr_t addr) { int shutdownResult; - + SubscriptionLock(); if(subscriptions == 0) shutdownResult = -1; else if ( subscriptions == 1) @@ -299,10 +314,24 @@ subscriptions--; shutdownResult = 0; } - + SubscriptionUnlock(); return( shutdownResult ); } +int Driver::Terminate() +{ + SubscriptionLock(); + if (subscriptions) + { + Shutdown(); + subscriptions = 0; + alwayson = 0; + InQueue->DataAvailable(); + } + SubscriptionUnlock(); + return 0; +} + /** @brief Wake up the driver if the specified event occurs on the file descriptor */ int Driver::AddFileWatch(int fd, bool ReadWatch , bool WriteWatch , bool ExceptWatch ) { @@ -315,64 +344,6 @@ return fileWatcher->RemoveFileWatch(fd,InQueue,ReadWatch,WriteWatch,ExceptWatch); } - -/* start a thread that will invoke Main() */ -void -Driver::StartThread(void) -{ - pthread_create(&driverthread, NULL, &DummyMain, this); -} - -/* cancel (and wait for termination) of the thread */ -void -Driver::StopThread(void) -{ - void* dummy; - pthread_cancel(driverthread); - // Release the driver thread, in case it's waiting on the message queue - // or the driver mutex. - this->InQueue->DataAvailable(); - this->Unlock(); - if(pthread_join(driverthread,&dummy)) - perror("Driver::StopThread:pthread_join()"); -} - -/* Dummy main (just calls real main) */ -void* -Driver::DummyMain(void *devicep) -{ - // Install a cleanup function - pthread_cleanup_push(&DummyMainQuit, devicep); - - // Run the overloaded Main() in the subclassed device. - ((Driver*)devicep)->Main(); - - // Run the uninstall cleanup function - pthread_cleanup_pop(1); - - return NULL; -} - -/* Dummy main cleanup (just calls real main cleanup) */ -void -Driver::DummyMainQuit(void *devicep) -{ - // Run the overloaded MainCleanup() in the subclassed device. - ((Driver*)devicep)->MainQuit(); -} - -void -Driver::Main() -{ - PLAYER_ERROR("You have called StartThread(), " - "but didn't provide your own Main()!"); -} - -void -Driver::MainQuit() -{ -} - // Default message handler int Driver::ProcessMessage(QueuePointer &resp_queue, player_msghdr * hdr, void * data) @@ -390,6 +361,7 @@ /// a message with no handler is reached void Driver::ProcessMessages(int maxmsgs) { + TestCancel(); // See if we have any pending messages and process them if(maxmsgs == 0) maxmsgs = this->InQueue->GetLength(); @@ -421,7 +393,7 @@ } } delete msg; - pthread_testcancel(); + TestCancel(); currmsg++; } } @@ -505,3 +477,8 @@ return RegisterProperty(prop->GetKey(), prop, cf, section); } +bool Driver::Wait(double TimeOut) +{ + bool ret = this->InQueue->Wait(TimeOut); + return ret; +} Modified: code/player/trunk/libplayercore/driver.h =================================================================== --- code/player/trunk/libplayercore/driver.h 2008-12-19 00:59:47 UTC (rev 7207) +++ code/player/trunk/libplayercore/driver.h 2008-12-19 02:10:56 UTC (rev 7208) @@ -89,46 +89,26 @@ /** @brief Base class for all drivers. -This class manages driver subscriptions, threads, and data -marshalling to/from device interfaces. All drivers inherit from -this class, and most will overload the Setup(), Shutdown() and -Main() methods. +This class manages driver subscriptions, and data +marshalling to/from device interfaces. Non threaded drivers inherit directly from +this class, and most will overload the Setup(), Shutdown() methods. */ class Driver { private: - /** @brief Mutex used to lock access, via Lock() and Unlock(), to - driver internals, like the list of subscribed queues. */ - pthread_mutex_t accessMutex; - /* @brief Last error value; useful for returning error codes from constructors. */ int error; PropertyBag propertyBag; + /** @brief Number of subscriptions to this driver. */ + int subscriptions; + public: + bool HasSubscriptions(); + protected: - /* @brief Start the driver thread - - This method is usually called from the overloaded Setup() method to - create the driver thread. This will call Main(). */ - virtual void StartThread(void); - - /** @brief Cancel (and wait for termination) of the driver thread - - This method is usually called from the overloaded Shutdown() method - to terminate the driver thread. */ - virtual void StopThread(void); - - /** @brief Dummy main (just calls real main). This is used to simplify - thread creation. */ - static void* DummyMain(void *driver); - - /** @brief Dummy main cleanup (just calls real main cleanup). This is - used to simplify thread termination. */ - static void DummyMainQuit(void *driver); - /** @brief Add an interface. @param addr Player device address. @@ -143,7 +123,7 @@ @param addr Pointer to Player device address, this is filled in with the address details. @returns 0 on success, non-zero otherwise. */ - int AddInterface(player_devaddr_t *addr, ConfigFile * cf, int section, int code, char * key = NULL); + int AddInterface(player_devaddr_t *addr, ConfigFile * cf, int section, int code, const char * key = NULL); /** @brief Set/reset error code */ @@ -158,7 +138,7 @@ If TimeOut is set to a positive value this method will return false if the timeout occurs before and update is recieved. */ - bool Wait(double TimeOut=0.0) { return this->InQueue->Wait(TimeOut); } + virtual bool Wait(double TimeOut=0.0); /** @brief Wake up the driver if the specified event occurs on the file descriptor */ int AddFileWatch(int fd, bool ReadWatch = true, bool WriteWatch = false, bool ExceptWatch = true); @@ -167,17 +147,31 @@ int RemoveFileWatch(int fd, bool ReadWatch = true, bool WriteWatch = false, bool ExceptWatch = true); - public: - /** @brief The driver's thread. - - The driver's thread, when managed by StartThread() and - StopThread(). */ - pthread_t driverthread; - /** @brief Lock access to driver internals. */ - virtual void Lock(void); + private: + /** @brief Mutex used to lock access, via Lock() and Unlock(), to + driver internals, like the list of subscribed queues. */ + pthread_mutex_t accessMutex; + /** @brief Mutex used to protect the subscription count for the driver. */ + pthread_mutex_t subscriptionMutex; + protected: + /** @brief Lock access between the server and driver threads. In particular used + * to procect the drivers thread pointer */ + virtual void Lock(void); /** @brief Unlock access to driver internals. */ virtual void Unlock(void); + + /** @brief Lock to protect the subscription count for the driver */ + virtual void SubscriptionLock(void); + /** @brief Unlock to protect the subscription count for the driver. */ + virtual void SubscriptionUnlock(void); + /** enable thread cancellation and test for cancellation + * + * This should only ever be called from the driver thread with *no* locks held*/ + virtual void TestCancel() {}; + + + public: /** @brief Last requester's queue. Pointer to a queue to which this driver owes a reply. Used mainly @@ -259,9 +253,6 @@ /** @brief Default device address (single-interface drivers) */ player_devaddr_t device_addr; - /** @brief Number of subscriptions to this driver. */ - int subscriptions; - /** @brief Total number of entries in the device table using this driver. This is updated and read by the Device class. */ int entries; @@ -362,35 +353,30 @@ @returns Returns 0 on success. */ virtual int Unsubscribe(QueuePointer &queue, player_devaddr_t addr) {return 1;}; + /** @brief Terminate the driver. + + This method doesnt go through the niceities of unsubscribing etc, only use + when the server is actually shutting down as it takes a few shortcuts + + @returns Returns 0 on success. */ + virtual int Terminate(); + + /** @brief Initialize the driver. This function is called with the first client subscribes; it MUST be implemented by the driver. @returns Returns 0 on success. */ - virtual int Setup() = 0; + virtual int Setup() {return 0;}; /** @brief Finalize the driver. - This function is called with the last client unsubscribes; it MUST - be implemented by the driver. + This function is called with the last client unsubscribes. @returns Returns 0 on success. */ - virtual int Shutdown() = 0; + virtual int Shutdown() {return 0;}; - /** @brief Main method for driver thread. - - drivers have their own thread of execution, created using - StartThread(); this is the entry point for the driver thread, - and must be overloaded by all threaded drivers. */ - virtual void Main(void); - - /** @brief Cleanup method for driver thread (called when main exits) - - Overload this method and to do additional cleanup when the - driver thread exits. */ - virtual void MainQuit(void); - /** @brief Process pending messages. Call this to automatically process messages using registered handler, @@ -421,7 +407,6 @@ /** @brief Update non-threaded drivers. */ virtual void Update() { - if(!this->driverthread) this->ProcessMessages(); } @@ -455,5 +440,174 @@ virtual bool RegisterProperty(Property *prop, ConfigFile* cf, int section); }; +typedef enum player_thread_state +{ + PLAYER_THREAD_STATE_STOPPED, + PLAYER_THREAD_STATE_RUNNING, + PLAYER_THREAD_STATE_STOPPING, + PLAYER_THREAD_STATE_RESTARTING +} player_thread_state_t; + + +/** +@brief Base class for drivers which oeprate with a thread. + +This class manages driver subscriptions, threads, and data +marshalling to/from device interfaces. All drivers inherit from +this class, and most will overload the MainSetup(), MainQuit() and +Main() methods. + +The ThreadedDriver uses a fairly simple thread model with the only complications +coming from the deferred cancellation. + +The default setup method simply calls StartThread and likewise the default Shutdown method calls +StopThread. Resources for the driver should be allocated in MainSetup and cleaned up in MainQuit, these +two methods will be called in the driver thread before and after the main method respectively. + +When StopThread is called it will request the driver thread to be cancelled, this cancellation will be +deffered until one of three cancellation points, ProcessMessages, Wait or TestCancel. In this way the driver +has control over when the thread is cancelled. + +If a new subscription arrives before the thread has terminated then upon completion of the MainQuit method the thread +will be relaunched and MainSetup run. In this way it is gaurunteed that there is only ever one thread running for +the driver and that Main is always proceeded by MainSetup and followed by MainQuit before any new threads are launched. + +To manage the thread lifetime a small state machine is implemented, containing four states: STOPPED, RUNNING, STOPPING +and RESTARTING. Initially the thread is in STOPPED state. + +From the STOPPED state the only valid transition is to RUNNING, this is triggered by a call to StartThread. + +From the RUNNING state the driver can transition to STOPPING, triggered by a call to StopThread. + +In the STOPPING state the driver can transition to STOPPED, triggered by MainQuit running, or to RESTARTING on a call to +StartThread. + +From RESTARTING the driver can transition to RUNNING once MainQuit has run and the new thread is started, or to STOPPING if +StopThread is called before the original thread terminates. + +*/ +class ThreadedDriver : public Driver +{ + protected: + + /* @brief Start the driver thread + + This method is usually called from the overloaded Setup() method to + create the driver thread. This will call Main(). */ + virtual void StartThread(void); + + /** @brief Cancel (and wait for termination) of the driver thread + + This method is usually called from the overloaded Shutdown() method + to terminate the driver thread. */ + virtual void StopThread(void); + + /** @brief Dummy main (just calls real main). This is used to simplify + thread creation. */ + static void* DummyMain(void *driver); + + /** @brief Dummy main cleanup (just calls real main cleanup). This is + used to simplify thread termination. */ + static void DummyMainQuit(void *driver); + + private: + /** @brief The driver's thread. + The driver's thread, when managed by StartThread() and + StopThread(). */ + pthread_t driverthread; + + /** @brief TODO: insert state machine here + . */ + player_thread_state_t ThreadState; + bool SetupSuccessful; + /** @brief barrier to make sure StartThread doesnt return until + * cleanup handlers etc have been installed.*/ + pthread_barrier_t threadSetupBarrier; + + protected: + /** enable thread cancellation and test for cancellation + * + * This should only ever be called from the driver thread with *no* locks held*/ + void TestCancel(); + + + public: + + /** @brief Constructor with implicit interface + @param cf Current configuration file + @param section Current section in configuration file + @param overwrite_cmds Do new commands overwrite old ones? + @param queue_maxlen How long can the incoming queue grow? + @param interface The interface that you want this driver to provide*/ + ThreadedDriver(ConfigFile *cf, + int section, + bool overwrite_cmds, + size_t queue_maxlen, + int interface); + + /** @brief Constructor for multiple-interface drivers. + + Use AddInterface() to specify individual interfaces. + @param cf Current configuration file + @param section Current section in configuration file + @param overwrite_cmds Do new commands overwrite old ones? + @param queue_maxlen How long can the incoming queue grow? */ + ThreadedDriver(ConfigFile *cf, + int section, + bool overwrite_cmds = true, + size_t queue_maxlen = PLAYER_MSGQUEUE_DEFAULT_MAXLEN); + + /** @brief Destructor */ + virtual ~ThreadedDriver(); + + /** @brief Initialize the driver. + + This function is called with the first client subscribes; it MUST + be implemented by the driver. + + @returns Returns 0 on success. */ + virtual int Setup(); + + /** @brief Finalize the driver. + + This function is called with the last client unsubscribes; the default version simple stops the + driver thread if one is present. In this case resources should be cleaned up in MainQuit. + + @returns Returns 0 on success. */ + virtual int Shutdown(); + + /** @brief Main method for driver thread. + + drivers have their own thread of execution, created using + StartThread(); this is the entry point for the driver thread, + and must be overloaded by all threaded drivers. */ + virtual void Main(void) = 0; + + /** @brief Sets up the resources needed by the driver thread */ + virtual int MainSetup(void) {return 0;}; + + /** @brief Cleanup method for driver thread (called when main exits) + + Overload this method and to do additional cleanup when the + driver thread exits. */ + virtual void MainQuit(void) {}; + + /** @brief Wait for new data to arrive on the driver's queue. + + Call this method to block until a new message arrives on + Driver::InQueue. This method will return true immediately if at least + one message is already waiting. + + If TimeOut is set to a positive value this method will return false if the + timeout occurs before and update is recieved. + */ + bool Wait(double TimeOut=0.0); + + virtual void Update() + {}; + +}; + + #endif Modified: code/player/trunk/server/drivers/actarray/amtecM5.cc =================================================================== --- code/player/trunk/server/drivers/actarray/amtecM5.cc 2008-12-19 00:59:47 UTC (rev 7207) +++ code/player/trunk/server/drivers/actarray/amtecM5.cc 2008-12-19 02:10:56 UTC (rev 7208) @@ -69,17 +69,13 @@ //////////////////////////////////////////// //This is the class for the Player Driver //////////////////////////////////////////// -class AmtecM5 : public Driver { +class AmtecM5 : public ThreadedDriver { public: // Constructor; need that AmtecM5(ConfigFile* cf, int section); ~AmtecM5(void); - // Must implement the following methods. - virtual int Setup(); - virtual int Shutdown(); - // This method will be invoked on each incoming message virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr * hdr, void * data); @@ -142,6 +138,9 @@ // Main function for device thread. virtual void Main(); + virtual void MainQuit(); + virtual int MainSetup(); + player_actarray_data_t actArray; player_actarray_actuator_t *actuators; player_actarray_actuatorgeom_t *actuatorsGeom; @@ -306,7 +305,7 @@ //////////////////////////////////////////////////////////////////////////////// // Set up the device. Return 0 if things go well, and -1 otherwise. // Usually runs the first time someone connects to the interface -int AmtecM5::Setup() { +int AmtecM5::MainSetup() { using namespace std; puts("Amtec M5 Powercube driver initialising"); @@ -478,25 +477,12 @@ puts("Amtec M5 powercube driver ready."); - // Start the device thread; spawns a new thread and executes - // AmtecM5::Main(), which contains the main loop for the driver. - StartThread(); - return(0); } -//////////////////////////////////////////////////////////////////////////////// -// Shutdown the device -int AmtecM5::Shutdown() { - puts("Shutting Amtec M5 Powercube driver down"); - - //Wait a little to let everything settle - sleep(1); //We've been getting some segfaults at disconnect from the last client without this - - // Stop and join the driver thread - StopThread(); - +void MainQuit() +{ if (debug_level) { cout << "About to delete the pclDevice.\n"; } Modified: code/player/trunk/server/drivers/audio/alsa.cc =================================================================== --- code/player/trunk/server/drivers/audio/alsa.cc 2008-12-19 00:59:47 UTC (rev 7207) +++ code/player/trunk/server/drivers/audio/alsa.cc 2008-12-19 02:10:56 UTC (rev 7208) @@ -2007,7 +2007,7 @@ // Constructor. Retrieve options from the configuration file and do any // pre-Setup() setup. Alsa::Alsa (ConfigFile* cf, int section) - : Driver (cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_AUDIO_CODE) + : ThreadedDriver (cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_AUDIO_CODE) { pbDevice = mixerDevice = recDevice = NULL; pbHandle = NULL; @@ -2153,7 +2153,7 @@ } // Set up the device. Return 0 if things go well, and -1 otherwise. -int Alsa::Setup (void) +int Alsa::MainSetup (void) { // Clear queue and set to initial values ClearQueue (); @@ -2191,16 +2191,11 @@ playState = PB_STATE_STOPPED; recState = PB_STATE_STOPPED; - StartThread (); return 0; } - -// Shutdown the device -int Alsa::Shutdown (void) +void Alsa::MainQuit() { - StopThread (); - // Clean up PCM file descriptors if (pbFDs) delete[] pbFDs; @@ -2252,8 +2247,6 @@ } } } - - return 0; } Modified: code/player/trunk/server/drivers/audio/alsa.h =================================================================== --- code/player/trunk/server/drivers/audio/alsa.h 2008-12-19 00:59:47 UTC (rev 7207) +++ code/player/trunk/server/drivers/audio/alsa.h 2008-12-19 02:10:56 UTC (rev 7208) @@ -79,15 +79,12 @@ //////////////////////////////////////////////////////////////////////////////// // The class for the driver -class Alsa : public Driver +class Alsa : public ThreadedDriver { public: Alsa (ConfigFile* cf, int section); ~Alsa (void); - virtual int Setup (void); - virtual int Shutdown (void); - virtual int ProcessMessage (QueuePointer &resp_queue, player_msghdr *hdr, void *data); private: @@ -144,6 +141,8 @@ // Internal functions virtual void Main (void); + virtual int MainSetup (void); + virtual void MainQuit (void); void SendStateMessage (void); // Stored sample functions Modified: code/player/trunk/server/drivers/base/imagebase.cc =================================================================== --- code/player/trunk/server/drivers/base/imagebase.cc 2008-12-19 00:59:47 UTC (rev 7207) +++ code/player/trunk/server/drivers/base/imagebase.cc 2008-12-19 02:10:56 UTC (rev 7208) @@ -41,7 +41,7 @@ // Constructor ImageBase::ImageBase(ConfigFile *cf, int section, bool overwrite_cmds, size_t queue_maxlen, int interf) - : Driver(cf, section, overwrite_cmds, queue_maxlen, interf) + : ThreadedDriver(cf, section, overwrite_cmds, queue_maxlen, interf) { memset(&this->camera_addr, 0, sizeof(player_devaddr_t)); stored_data.image = NULL; @@ -60,7 +60,7 @@ ImageBase::ImageBase(ConfigFile *cf, int section, bool overwrite_cmds, size_t queue_maxlen) - : Driver(cf, section, overwrite_cmds, queue_maxlen) + : ThreadedDriver(cf, section, overwrite_cmds, queue_maxlen) { memset(&this->camera_addr, 0, sizeof(player_devaddr_t)); stored_data.image = NULL; @@ -80,7 +80,7 @@ //////////////////////////////////////////////////////////////////////////////// // Set up the device (called by server thread). -int ImageBase::Setup() +int ImageBase::MainSetup() { // Subscribe to the camera. if (Device::MatchDeviceAddress (camera_addr, device_addr)) @@ -99,17 +99,7 @@ return -1; } - StartThread(); - return 0; -} - -//////////////////////////////////////////////////////////////////////////////// -// Shutdown the device (called by server thread). -int ImageBase::Shutdown() -{ - StopThread(); - camera_driver->Unsubscribe(InQueue); return 0; @@ -127,7 +117,6 @@ { assert(data); player_camera_data_t * compdata = reinterpret_cast<player_camera_data_t *>(data); - Lock(); if (!HaveData) { this->stored_data.width = (compdata->width); @@ -186,7 +175,6 @@ #endif HaveData = true; } - Unlock(); return 0; } return -1; @@ -202,15 +190,11 @@ ProcessMessages(); - Lock(); if (HaveData) { - Unlock(); ProcessFrame(); - Lock(); HaveData = false; } - Unlock(); } } Modified: code/player/trunk/server/drivers/base/imagebase.h =================================================================== --- code/player/trunk/server/drivers/base/imagebase.h 2008-12-19 00:59:47 UTC (rev 7207) +++ code/player/trunk/server/drivers/base/imagebase.h 2008-12-19 02:10:56 UTC (rev 7208) @@ -59,7 +59,7 @@ #include <libplayercore/error.h> // Driver for detecting laser retro-reflectors. -class ImageBase : public Driver +class ImageBase : public ThreadedDriver { public: // Constructor @@ -71,10 +71,6 @@ PLAYER_WARN("image deleted from the memory"); } - // Setup/shutdown routines. - virtual int Setup(); - virtual int Shutdown(); - // Process incoming messages from clients int ProcessMessage (QueuePointer &resp_queue, player_msghdr * hdr, void * data); @@ -84,8 +80,9 @@ protected: virtual int ProcessFrame() = 0; - // Main function for device thread. + // Main functions for device thread. virtual void Main(); + virtual int MainSetup(); // Input camera stuff Device *camera_driver; Modified: code/player/trunk/server/drivers/blackboard/localbb/localbb.cpp =================================================================== --- code/player/trunk/server/drivers/blackboard/localbb/localbb.cpp 2008-12-19 00:59:47 UTC (rev 7207) +++ code/player/trunk/server/drivers/blackboard/localbb/localbb.cpp 2008-12-19 02:10:56 UTC (rev 7208) @@ -353,22 +353,8 @@ qps.erase(*ltr); } } - int shutdownResult; - - if(subscriptions == 0) - shutdownResult = -1; - else if ( subscriptions == 1) - { - shutdownResult = Shutdown(); - subscriptions--; - } - else - { - subscriptions--; - shutdownResult = 0; - } - - return shutdownResult; + + return Driver::Unsubscribe(addr); } //////////////////////////////////////////////////////////////////////////////// // Factory method. Modified: code/player/trunk/server/drivers/blobfinder/acts/acts.cc =================================================================== --- code/player/trunk/server/drivers/blobfinder/acts/acts.cc 2008-12-19 00:59:47 UTC (rev 7207) +++ code/player/trunk/server/drivers/blobfinder/acts/acts.cc 2008-12-19 02:10:56 UTC (rev 7208) @@ -246,7 +246,7 @@ -class Acts:public Driver +class Acts:public ThreadedDriver { private: int debuglevel; // debuglevel 0=none, 1=basic, 2=everything @@ -299,11 +299,11 @@ Acts( ConfigFile* cf, int section); virtual void Main(); - + virtual void MainQuit(); + void KillACTS(); - int Setup(); - int Shutdown(); + int MainSetup(); }; // a factory creation function @@ -333,7 +333,7 @@ Acts::Acts( ConfigFile* cf, int section) - : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_BLOBFINDER_CODE) + : ThreadedDriver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_BLOBFINDER_CODE) { char tmpstr[MAX_FILENAME_SIZE]; int tmpint; @@ -469,7 +469,7 @@ } int -Acts::Setup() +Acts::MainSetup() { int i; int j; @@ -742,28 +742,15 @@ } puts("Done."); - /* now spawn reading thread */ - StartThread(); - return(0); } - - // shut up compiler! return(0); } -int -Acts::Shutdown() +void Acts::MainQuit() { - /* if Setup() was never called, don't do anything */ - if(sock == -1) - return(0); - - StopThread(); - - sock = -1; - puts("ACTS vision server has been shutdown"); - return(0); + sock = -1; + puts("ACTS vision server has been shutdown"); } void Modified: code/player/trunk/server/drivers/blobfinder/cmvision/P2CMV.cc =================================================================== --- code/player/trunk/server/drivers/blobfinder/cmvision/P2CMV.cc 2008-12-19 00:59:47 UTC (rev 7207) +++ code/player/trunk/server/drivers/blobfinder/cmvision/P2CMV.cc 2008-12-19 02:10:56 UTC (rev 7208) @@ -135,7 +135,7 @@ /********************************************************************/ -class CMVisionBF: public Driver +class CMVisionBF: public ThreadedDriver { private: int mDebugLevel; // debuglevel 0=none, @@ -160,8 +160,7 @@ void Print(); public: - int Setup(); - int Shutdown(); + // constructor CMVisionBF(ConfigFile* cf, int section); virtual ~CMVisionBF(); @@ -170,6 +169,8 @@ player_msghdr * hdr, void * data); virtual void Main(); + int MainSetup(); + void ProcessImageData(); }; @@ -188,7 +189,7 @@ } CMVisionBF::CMVisionBF( ConfigFile* cf, int section) - : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, + : ThreadedDriver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_BLOBFINDER_CODE), mWidth(0), mHeight(0), @@ -217,7 +218,7 @@ } int -CMVisionBF::Setup() +CMVisionBF::MainSetup() { if (mVision) { @@ -243,20 +244,6 @@ memset(&mData,0,sizeof(mData)); allocated_blobs = 0; puts("done."); - - StartThread(); - return(0); -} - -int -CMVisionBF::Shutdown() -{ - /* if Setup() was never called, don't do anything */ - if (mVision==NULL) - return 0; - - StopThread(); - // Unsubscribe from the camera this->mCameraDev->Unsubscribe(this->InQueue); @@ -420,7 +407,6 @@ if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA, PLAYER_CAMERA_DATA_STATE, this->mCameraAddr)) { - // Lock(); // we can't quite do this so easily with camera data // because the images are different than the max size //assert(hdr->size == sizeof(player_camera_data_t)); @@ -468,7 +454,6 @@ // we have a new image, ProcessImageData(); } - // Unlock(); return(0); } Modified: code/player/trunk/server/drivers/camera/1394/camera1394.cc =================================================================== --- code/player/trunk/server/drivers/camera/1394/camera1394.cc 2008-12-19 00:59:47 UTC (rev 7207) +++ code/player/trunk/server/drivers/camera/1394/camera1394.cc 2008-12-19 02:10:56 UTC (rev 7208) @@ -284,19 +284,20 @@ #endif // Driver for detecting laser retro-reflectors. -class Camera1394 : public Driver +class Camera1394 : public ThreadedDriver { // Constructor public: Camera1394( ConfigFile* cf, int section); // Setup/shutdown routines. - public: virtual int Setup(); - public: virtual int Shutdown(); private: void SafeCleanup(); // Main function for device thread. private: virtual void Main(); + private: virtual void MainQuit(); + public: virtual int MainSetup(); + // This method will be invoked on each incoming message public: virtual int ProcessMessage(QueuePointer & resp_queue, player_msghdr * hdr, @@ -390,7 +391,7 @@ //////////////////////////////////////////////////////////////////////////////// // Constructor Camera1394::Camera1394(ConfigFile* cf, int section) - : Driver(cf, + : ThreadedDriver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, @@ -742,7 +743,7 @@ //////////////////////////////////////////////////////////////////////////////// // Set up the device (called by server thread). -int Camera1394::Setup() +int Camera1394::MainSetup() { @@ -1071,20 +1072,11 @@ this->SafeCleanup(); return -1; } - - // Start the driver thread. - this->StartThread(); - return 0; } - -//////////////////////////////////////////////////////////////////////////////// -// Shutdown the device (called by server thread). -int Camera1394::Shutdown() +void Camera1394::MainQuit() { - // Stop the driver thread. - StopThread(); // Stop transmitting camera data #if LIBDC1394_VERSION == 0200 @@ -1097,8 +1089,6 @@ // Free resources this->SafeCleanup(); - - return 0; } Modified: code/player/trunk/server/drivers/camera/compress/cameracompress.cc =================================================================== --- code/player/trunk/server/drivers/camera/compress/cameracompress.cc 2008-12-19 00:59:47 UTC (rev 7207) +++ code/player/trunk/server/drivers/camera/compress/cameracompress.cc 2008-12-19 02:10:56 UTC (rev 7208) @@ -85,13 +85,13 @@ #include <libplayercore/error.h> #include <libplayerjpeg/playerjpeg.h> -class CameraCompress : public Driver +class CameraCompress : public ThreadedDriver { // Constructor public: CameraCompress( ConfigFile* cf, int section); // Setup/shutdown routines. - public: virtual int Setup(); + public: virtual int MainSetup(); public: virtual int Shutdown(); // This method will be invoked on each incoming message @@ -136,7 +136,7 @@ } CameraCompress::CameraCompress( ConfigFile *cf, int section) - : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_CAMERA_CODE) + : ThreadedDriver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_CAMERA_CODE) { this->data.image = NULL; this->frameno = 0; @@ -157,7 +157,7 @@ return; } -int CameraCompress::Setup() +int CameraCompress::MainSetup() { // Subscribe to the camera. if(Device::MatchDeviceAddress(this->camera_id, this->device_addr)) @@ -176,9 +176,6 @@ return(-1); } - // Start the driver thread. - this->StartThread(); - return 0; } Modified: code/player/trunk/server/drivers/camera/compress/camerauncompress.cc =================================================================== --- code/player/trunk/server/drivers/camera/compress/camerauncompress.cc 2008-12-19 00:59:47 UTC (rev 7207) +++ code/player/trunk/server/drivers/camera/compress/camerauncompress.cc 2008-12-19 02:10:56 UTC (rev 7208) @@ -86,13 +86,13 @@ #include <libplayercore/error.h> #include <libplayerjpeg/playerjpeg.h> -class CameraUncompress : public Driver +class CameraUncompress : public ThreadedDriver { // Constructor public: CameraUncompress( ConfigFile* cf, int section); // Setup/shutdown routines. - public: virtual int Setup(); + public: virtual int MainSetup(); public: virtual int Shutdown(); // This method will be invoked on each incoming message @@ -138,7 +138,7 @@ } CameraUncompress::CameraUncompress( ConfigFile *cf, int section) - : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_CAMERA_CODE) + : ThreadedDriver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_CAMERA_CODE) { this->frameno = 0; @@ -157,7 +157,7 @@ return; } -int CameraUncompress::Setup() +int CameraUncompress::MainSetup() { // Subscribe to the laser. if(Device::MatchDeviceAddress(this->camera_id, this->device_addr)) @@ -176,9 +176,6 @@ return(-1); } - // Start the driver thread. - this->StartThread(); - return 0; } Modified: code/player/trunk/server/drivers/camera/cvcam/cvcam.cc =================================================================== --- code/player/trunk/server/drivers/camera/cvcam/cvcam.cc 2008-12-19 00:59:47 UTC (rev 7207) +++ code/player/trunk/server/drivers/camera/cvcam/cvcam.cc 2008-12-19 02:10:56 UTC (rev 7208) @@ -92,13 +92,13 @@ //--------------------------------- -class CvCam : public Driver +class CvCam : public ThreadedDriver { public: CvCam(ConfigFile * cf, int section); public: virtual ~CvCam(); - public: virtual int Setup(); - public: virtual int Shutdown(); + public: virtual int MainSetup(); + public: virtual void MainQuit(); // This method will be invoked on each incoming message public: virtual int ProcessMessage(QueuePointer & resp_queue, @@ -125,7 +125,7 @@ } CvCam::CvCam(ConfigFile * cf, int section) - : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_CAMERA_CODE) + : ThreadedDriver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_CAMERA_CODE) { this->capture = NULL; this->camindex = cf->ReadInt(section, "camindex", CV_CAP_ANY); @@ -145,7 +145,7 @@ if (this->capture) cvReleaseCapture(&(this->capture)); } -int CvCam::Setup() +int CvCam::MainSetup() { if (this->capture) cvReleaseCapture(&(this->capture)); this->capture = cvCaptureFromCAM(this->camindex); @@ -154,16 +154,13 @@ PLAYER_ERROR("Couldn't create capture device. Something is wrong with your OpenCV."); return -1; } - this->StartThread(); return 0; } -int CvCam::Shutdown() +int CvCam::MainQuit() { - StopThread(); if (this->capture) cvReleaseCapture(&(this->capture)); this->capture = NULL; - return 0; } void CvCam::Main() Modified: code/player/trunk/server/drivers/camera/imageseq/imageseq.cc =================================================================== --- code/player/trunk/server/drivers/camera/imageseq/imageseq.cc 2008-12-19 00:59:47 UTC (rev 7207) +++ code/player/trunk/server/drivers/camera/imageseq/imageseq.cc 2008-12-19 02:10:56 UTC (rev 7208) @@ -98,15 +98,11 @@ #include <libplayercore/playercore.h> -class ImageSeq : public Driver +class ImageSeq : public ThreadedDriver { // Constructor public: ImageSeq( ConfigFile *cf, int section); - // Setup/shutdown routines. - public: virtual int Setup(); - public: virtual int Shutdown(); - // Main function for device thread. private: virtual void Main(); @@ -146,7 +142,7 @@ ///////////////////////////////////////... [truncated message content] |
From: <th...@us...> - 2008-12-19 02:50:16
|
Revision: 7210 http://playerstage.svn.sourceforge.net/playerstage/?rev=7210&view=rev Author: thjc Date: 2008-12-19 02:50:12 +0000 (Fri, 19 Dec 2008) Log Message: ----------- Fixes for: Player server shutdown logic Reduced locking time for Filewatcher so other threads get a chance to add watches Unsubscription from localbb was removing random subscriptions Modified Paths: -------------- code/player/trunk/libplayercore/devicetable.cc code/player/trunk/libplayercore/devicetable.h code/player/trunk/libplayercore/driver.h code/player/trunk/libplayercore/error.h code/player/trunk/libplayercore/filewatcher.cc code/player/trunk/libplayercore/globals.cc code/player/trunk/libplayercore/message.cc code/player/trunk/libplayercore/threaded_driver.cc code/player/trunk/server/drivers/blackboard/localbb/localbb.cpp code/player/trunk/server/drivers/position/nav200/sicknav200.cc code/player/trunk/server/server.cc Modified: code/player/trunk/libplayercore/devicetable.cc =================================================================== --- code/player/trunk/libplayercore/devicetable.cc 2008-12-19 02:36:48 UTC (rev 7209) +++ code/player/trunk/libplayercore/devicetable.cc 2008-12-19 02:50:12 UTC (rev 7210) @@ -272,6 +272,29 @@ return(0); } +int +DeviceTable::StopAlwaysonDrivers() +{ + Device* thisentry; + + // We don't lock here, on the assumption that the caller is also the only + // thread that can make changes to the device table. + for(thisentry=head;thisentry;thisentry=thisentry->next) + { + if(thisentry->driver->alwayson) + { + QueuePointer Temp = QueuePointer(); + if(thisentry->Unsubscribe(Temp) != 0) + { + PLAYER_ERROR2("alwayson unsubscription failed for device %s:%d", + interf_to_str(thisentry->addr.interf), thisentry->addr.index); + return(-1); + } + } + } + return(0); +} + // Register a factory creation function. It will be called when // GetDevice fails to find a device in the deviceTable. This function // might, for example, locate the device on a remote host (in a Modified: code/player/trunk/libplayercore/devicetable.h =================================================================== --- code/player/trunk/libplayercore/devicetable.h 2008-12-19 02:36:48 UTC (rev 7209) +++ code/player/trunk/libplayercore/devicetable.h 2008-12-19 02:50:12 UTC (rev 7210) @@ -109,6 +109,15 @@ // than just driver-specific. int StartAlwaysonDrivers(); + // Unsubscribe to each device whose driver is marked 'alwayson'. Returns + // 0 on success, -1 on error (at least one driver failed to start). + // + // Only used for coordinating server shutdown + // + // TODO: change the semantics of alwayson to be device-specific, rather + // than just driver-specific. + int StopAlwaysonDrivers(); + // Register a factory creation function. It will be called when // GetDevice fails to find a device in the deviceTable. This function // might, for example, locate the device on a remote host (in a Modified: code/player/trunk/libplayercore/driver.h =================================================================== --- code/player/trunk/libplayercore/driver.h 2008-12-19 02:36:48 UTC (rev 7209) +++ code/player/trunk/libplayercore/driver.h 2008-12-19 02:50:12 UTC (rev 7210) @@ -577,6 +577,14 @@ @returns Returns 0 on success. */ virtual int Shutdown(); + /** @brief Terminate the driver. + + This method doesnt go through the niceities of unsubscribing etc, only use + when the server is actually shutting down as it takes a few shortcuts + + @returns Returns 0 on success. */ + virtual int Terminate(); + /** @brief Main method for driver thread. drivers have their own thread of execution, created using Modified: code/player/trunk/libplayercore/error.h =================================================================== --- code/player/trunk/libplayercore/error.h 2008-12-19 02:36:48 UTC (rev 7209) +++ code/player/trunk/libplayercore/error.h 2008-12-19 02:50:12 UTC (rev 7210) @@ -66,6 +66,7 @@ #define PLAYER_ERROR2(msg, a, b) ErrorPrint(PLAYER_ERR_ERR, 0, __FILE__, __LINE__, "error : " msg "\n", a, b) #define PLAYER_ERROR3(msg, a, b, c) ErrorPrint(PLAYER_ERR_ERR, 0, __FILE__, __LINE__, "error : " msg "\n", a, b, c) #define PLAYER_ERROR4(msg, a, b, c,d) ErrorPrint(PLAYER_ERR_ERR, 0, __FILE__, __LINE__, "error : " msg "\n", a, b, c, d) +#define PLAYER_ERROR5(msg, a, b, c, d, e) ErrorPrint(PLAYER_ERR_ERR, 0, __FILE__, __LINE__, "error : " msg "\n", a, b, c, d, e) /// Warning message macros #define PLAYER_WARN(msg) ErrorPrint(PLAYER_ERR_WARN, 0, __FILE__, __LINE__, "warning : " msg "\n") Modified: code/player/trunk/libplayercore/filewatcher.cc =================================================================== --- code/player/trunk/libplayercore/filewatcher.cc 2008-12-19 02:36:48 UTC (rev 7209) +++ code/player/trunk/libplayercore/filewatcher.cc 2008-12-19 02:50:12 UTC (rev 7210) @@ -77,20 +77,25 @@ t.tv_sec = static_cast<int> (floor(Timeout)); t.tv_usec = static_cast<int> ((Timeout - static_cast<int> (floor(Timeout))) * 1e6); + // unlock for the select call + // in the worst case if the list gets modified during the call we will either + // not be able to match an event on a deleted fd, or will get spurious wake ups + // on a newly added fd all of which are non fatal + Unlock(); int ret = select (maxfd+1,&ReadFds,&WriteFds,&ExceptFds,&t); if (ret < 0) { PLAYER_ERROR2("Select called failed in File Watcher: %d %s",errno,strerror(errno)); - Unlock(); return ret; } else if (ret == 0) { - Unlock(); return 0; } + Lock(); + int queueless_count = 0; int match_count = 0; Modified: code/player/trunk/libplayercore/globals.cc =================================================================== --- code/player/trunk/libplayercore/globals.cc 2008-12-19 02:36:48 UTC (rev 7209) +++ code/player/trunk/libplayercore/globals.cc 2008-12-19 02:50:12 UTC (rev 7210) @@ -74,7 +74,7 @@ bool player_quit; bool player_quiet_startup; -// global access to the cmdlihe arguments +// global access to the cmdline arguments int player_argc; char** player_argv; Modified: code/player/trunk/libplayercore/message.cc =================================================================== --- code/player/trunk/libplayercore/message.cc 2008-12-19 02:36:48 UTC (rev 7209) +++ code/player/trunk/libplayercore/message.cc 2008-12-19 02:50:12 UTC (rev 7210) @@ -132,7 +132,8 @@ } else { - PLAYER_ERROR3 ("failed to find clone function for message %s: %s, %d", interf_to_str (Header.addr.interf), msgtype_to_str (Header.type), Header.subtype); + this->Data = NULL; + PLAYER_ERROR5 ("failed to find clone function for message %s[%d]: %s[%d], %d", interf_to_str (Header.addr.interf),Header.addr.interf, msgtype_to_str (Header.type), Header.type, Header.subtype); } } else @@ -529,8 +530,8 @@ // If it was a response, then mark it , to prompt // processing of the queue. if(!this->data_requested && - (hdr->type == PLAYER_MSGTYPE_RESP_ACK) || - (hdr->type == PLAYER_MSGTYPE_RESP_NACK)) + (hdr->type == PLAYER_MSGTYPE_RESP_ACK || + hdr->type == PLAYER_MSGTYPE_RESP_NACK )) this->SetDataRequested(true,true); this->Unlock(); Modified: code/player/trunk/libplayercore/threaded_driver.cc =================================================================== --- code/player/trunk/libplayercore/threaded_driver.cc 2008-12-19 02:36:48 UTC (rev 7209) +++ code/player/trunk/libplayercore/threaded_driver.cc 2008-12-19 02:50:12 UTC (rev 7210) @@ -89,6 +89,17 @@ // destructor, to free up allocated queue. ThreadedDriver::~ThreadedDriver() { + // if our thread is still running request it to be stopped + if (ThreadState == PLAYER_THREAD_STATE_RUNNING || ThreadState == PLAYER_THREAD_STATE_RESTARTING) + { + StopThread(); + } + // wait for the thread to actually stop + while (ThreadState != PLAYER_THREAD_STATE_STOPPED) + { + usleep(100000); + } + pthread_barrier_destroy(&threadSetupBarrier); } @@ -129,6 +140,7 @@ { if (ThreadState == PLAYER_THREAD_STATE_RUNNING) { + PLAYER_MSG2(5,"Cancelling thread %p belonging to driver %p",driverthread,this); pthread_cancel(driverthread); if(pthread_detach(driverthread)) perror("ThreadedDriver::StopThread:pthread_detach()"); @@ -224,6 +236,16 @@ return 0; } +int ThreadedDriver::Terminate() +{ + int ret = Driver::Terminate(); + // wait for the thread to actually stop + while (ret == 0 && ThreadState != PLAYER_THREAD_STATE_STOPPED) + { + usleep(100000); + } + return ret; +} bool ThreadedDriver::Wait(double TimeOut) { Modified: code/player/trunk/server/drivers/blackboard/localbb/localbb.cpp =================================================================== --- code/player/trunk/server/drivers/blackboard/localbb/localbb.cpp 2008-12-19 02:36:48 UTC (rev 7209) +++ code/player/trunk/server/drivers/blackboard/localbb/localbb.cpp 2008-12-19 02:50:12 UTC (rev 7210) @@ -185,7 +185,7 @@ /** @brief Driver de-initialisation. */ int Shutdown(); /** Override the unsubscribe to stop sending events to devices which are no longer subscribed. */ - int Unsubscribe(player_devaddr_t addr); + int Unsubscribe(QueuePointer &queue, player_devaddr_t addr); // MessageHandler /** @brief Process incoming messages. @@ -307,15 +307,11 @@ * map<group, vector<device queue> > */ map<std::string, vector<QueuePointer> > group_listeners; - - /** Map of queues to devices. Used to remove unneeded queues when a device is unsubscribed. */ - map<player_devaddr_t, QueuePointer> subscribed_devices; }; //////////////////////////////////////////////////////////////////////////////// // Override the unsubscribe. Stop sending out events to unsubscribed devices. -int LocalBB::Unsubscribe(player_devaddr_t addr) +int LocalBB::Unsubscribe(QueuePointer &qp, player_devaddr_t addr) { - QueuePointer &qp = subscribed_devices[addr]; for (map<std::string, map<std::string, vector<QueuePointer> > >::iterator itr = listeners.begin(); itr != listeners.end(); itr++) { map<std::string, vector<QueuePointer> > &keys = (*itr).second; @@ -354,7 +350,7 @@ } } - return Driver::Unsubscribe(addr); + return Driver::Unsubscribe(qp,addr); } //////////////////////////////////////////////////////////////////////////////// // Factory method. @@ -676,7 +672,6 @@ BlackBoardEntry LocalBB::SubscribeKey(const std::string &key, const std::string &group, const QueuePointer &resp_queue, const player_devaddr_t addr) { listeners[group][key].push_back(resp_queue); - subscribed_devices[addr] = resp_queue; BlackBoardEntry entry = entries[group][key]; if (entry.key == "") { @@ -707,7 +702,6 @@ vector<BlackBoardEntry> LocalBB::SubscribeGroup(const std::string &group, const QueuePointer &qp, const player_devaddr_t addr) { group_listeners[group].push_back(qp); - subscribed_devices[addr] = qp; vector<BlackBoardEntry> group_entries; //map<group, map<key, entry> > Modified: code/player/trunk/server/drivers/position/nav200/sicknav200.cc =================================================================== --- code/player/trunk/server/drivers/position/nav200/sicknav200.cc 2008-12-19 02:36:48 UTC (rev 7209) +++ code/player/trunk/server/drivers/position/nav200/sicknav200.cc 2008-12-19 02:50:12 UTC (rev 7210) @@ -968,7 +968,7 @@ uint32_t type = *reinterpret_cast<uint32_t*>(wkb + 1); bool extendedWKB = type >> 24 == 32; // Extended WKBs seem to store a flag in the type variable. int headerSize = extendedWKB ? wkbHeaderSize + 4 : wkbHeaderSize; - if (type & 0xffffff != 4) // Ignore the most significant byte, it might have a flag in. + if ((type & 0xffffff) != 4) // Ignore the most significant byte, it might have a flag in. { PLAYER_WARN1( "InterpretLayerData only supports MultiPoint data %d\n", Modified: code/player/trunk/server/server.cc =================================================================== --- code/player/trunk/server/server.cc 2008-12-19 02:36:48 UTC (rev 7209) +++ code/player/trunk/server/server.cc 2008-12-19 02:50:12 UTC (rev 7210) @@ -347,6 +347,12 @@ { delete ptcp; delete pudp; + + if(deviceTable->StopAlwaysonDrivers() != 0) + { + PLAYER_ERROR("failed to stop alwayson drivers"); + } + player_globals_fini(); delete cf; } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2008-12-28 02:55:20
|
Revision: 7214 http://playerstage.svn.sourceforge.net/playerstage/?rev=7214&view=rev Author: thjc Date: 2008-12-28 01:32:24 +0000 (Sun, 28 Dec 2008) Log Message: ----------- Merge from 2-1 7087: make tcpremotedriver not replace messages (this should be left up to the remote driver) make passthrough less verbose Modified Paths: -------------- code/player/trunk/libplayertcp/playertcp.cc Property Changed: ---------------- code/player/trunk/ Property changes on: code/player/trunk ___________________________________________________________________ Modified: svn:mergeinfo - /code/player/branches/release-2-1-patches:6672-6673,6738,6834,6886,6905,6930,6936,6946,6951,6955,6975,6979-6981,6985,6987-6996,7204 /code/player/trunk:6985 + /code/player/branches/release-2-1-patches:6672-6673,6738,6834,6886,6905,6930,6936,6946,6951,6955,6975,6979-6981,6985,6987-6996,7087,7204 /code/player/trunk:6985 Modified: code/player/trunk/libplayertcp/playertcp.cc =================================================================== --- code/player/trunk/libplayertcp/playertcp.cc 2008-12-28 01:26:08 UTC (rev 7213) +++ code/player/trunk/libplayertcp/playertcp.cc 2008-12-28 01:32:24 UTC (rev 7214) @@ -256,7 +256,7 @@ // Create an outgoing queue for this client this->clients[j].queue = - QueuePointer(true,PLAYER_MSGQUEUE_DEFAULT_MAXLEN); + QueuePointer(false,PLAYER_MSGQUEUE_DEFAULT_MAXLEN); // Create a buffer to hold incoming messages this->clients[j].readbuffersize = PLAYERTCP_READBUFFER_SIZE; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |