Revision: 7259 http://playerstage.svn.sourceforge.net/playerstage/?rev=7259&view=rev Author: thjc Date: 2009-01-09 21:14:24 +0000 (Fri, 09 Jan 2009)
Log Message: ----------- Applied patch 2101864: New driver for the sick LD MRS (Ibeo LUX) Modified to work with threaded driver Modified Paths: -------------- code/player/trunk/client_libs/libplayerc/dev_laser.c code/player/trunk/libplayercore/interfaces/006_laser.def code/player/trunk/server/drivers/laser/CMakeLists.txt code/player/trunk/server/drivers/shell/readlog.cc code/player/trunk/server/drivers/shell/writelog.cc Added Paths: ----------- code/player/trunk/server/drivers/laser/sickLDMRS.cc Modified: code/player/trunk/client_libs/libplayerc/dev_laser.c =================================================================== --- code/player/trunk/client_libs/libplayerc/dev_laser.c 2009-01-09 21:03:45 UTC (rev 7258) +++ code/player/trunk/client_libs/libplayerc/dev_laser.c 2009-01-09 21:14:24 UTC (rev 7259) @@ -193,6 +193,40 @@ device->robot_pose[1] = scan_data->pose.py; device->robot_pose[2] = scan_data->pose.pa; } + else if((header->type == PLAYER_MSGTYPE_DATA) && + (header->subtype == PLAYER_LASER_DATA_SCANANGLE)) + { + player_laser_data_scanangle_t* scan_data = (player_laser_data_scanangle_t*)data; + + device->max_range = scan_data->max_range; + device->min_left = device->max_range; + device->min_right = device->max_range; + + device->scan_count = scan_data->ranges_count; + playerc_laser_reallocate_scans(device); + + for (i = 0; i < scan_data->ranges_count; i++) + { + r = scan_data->ranges[i]; + assert(r >= 0); + b = scan_data->angles[i]; + device->ranges[i] = r; + device->scan[i][0] = r; + device->scan[i][1] = b; + device->point[i].px = r * cos(b); + device->point[i].py = r * sin(b); + + if((i <= scan_data->ranges_count/2) && (r < device->min_right)) + device->min_right = r; + else if((i > scan_data->ranges_count/2) && (r < device->min_left)) + device->min_left = r; + } + for (i = 0; i < scan_data->intensity_count; i++) + device->intensity[i] = scan_data->intensity[i]; + + device->scan_id = scan_data->id; + } + else PLAYERC_WARN2("skipping laser message with unknown type/subtype: %s/%d\n", msgtype_to_str(header->type), header->subtype); Modified: code/player/trunk/libplayercore/interfaces/006_laser.def =================================================================== --- code/player/trunk/libplayercore/interfaces/006_laser.def 2009-01-09 21:03:45 UTC (rev 7258) +++ code/player/trunk/libplayercore/interfaces/006_laser.def 2009-01-09 21:14:24 UTC (rev 7259) @@ -21,6 +21,8 @@ message { DATA, SCAN, 1, player_laser_data_t }; /** Data subtype: pose-stamped scan */ message { DATA, SCANPOSE, 2, player_laser_data_scanpose_t }; +/** Data subtype: angle-stamped scan */ +message { DATA, SCANANGLE, 3, player_laser_data_scanangle_t }; /** Request/reply subtype: get geometry */ message { REQ, GET_GEOM, 1, player_laser_geom_t }; @@ -79,6 +81,30 @@ player_pose2d_t pose; } player_laser_data_scanpose_t; +/** @brief Data: scan (@ref PLAYER_LASER_DATA_SCANANGLE) + +The basic laser angle stamped data packet. */ +typedef struct player_laser_data_scanangle +{ + /** Maximum range [m]. */ + float max_range; + /** Number of range readings. */ + uint32_t ranges_count; + /** Range readings [m]. */ + float *ranges; + /** Number of agle readings. */ + uint32_t angles_count; + /** Angle readings [m]. */ + float *angles; + /** Number of intensity readings */ + uint32_t intensity_count; + /** Intensity readings. */ + uint8_t *intensity; + /** A unique, increasing, ID for the scan */ + uint32_t id; +} player_laser_data_scanangle_t; + + /** @brief Request/reply: Get geometry. The laser geometry (position and size) can be queried by sending a Modified: code/player/trunk/server/drivers/laser/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/laser/CMakeLists.txt 2009-01-09 21:03:45 UTC (rev 7258) +++ code/player/trunk/server/drivers/laser/CMakeLists.txt 2009-01-09 21:14:24 UTC (rev 7259) @@ -43,3 +43,7 @@ PLAYERDRIVER_OPTION (rs4leuze build_rs4leuze ON) PLAYERDRIVER_ADD_DRIVER (rs4leuze build_rs4leuze SOURCES RS4LeuzeLaserDriver.cc RS4Leuze_laser.cc) + +PLAYERDRIVER_OPTION (sickLDMRS build_sickLDMRS ON) +PLAYERDRIVER_ADD_DRIVER (sickLDMRS build_sickLDMRS SOURCES sickLDMRS.cc) + Added: code/player/trunk/server/drivers/laser/sickLDMRS.cc =================================================================== --- code/player/trunk/server/drivers/laser/sickLDMRS.cc (rev 0) +++ code/player/trunk/server/drivers/laser/sickLDMRS.cc 2009-01-09 21:14:24 UTC (rev 7259) @@ -0,0 +1,539 @@ +/** @ingroup drivers Drivers */ +/** @{ */ +/** @defgroup driver_sickLDMRS sickLDMRS + * @brief SICK LD MRS / Multi plae and multi return laser scanner + +This driver is for communicting with the Ibeo LUX laser scanner over ethernet using the tcpstream driver + +...@par Provides + +- laser + +...@par Requires + + - opaque + +...@par Supported commands + +- none + +...@par Supported configuration requests + + - none + +...@par Configuration file options + + - buffer_size (int) + - default: 100000 + - The size of the buffer in the lux driver + - layer (int) + - -1(default) = all layers + - 0,1,2,3 = only publish layer x of the data (3 = highest) + - echo (int) + - -1(default) = all echos + - 0,1,2,3 = only publish laser returns which are echo x + - intensity (0,1,2,3) + - 1(default) - publishes the echo width (in cm) as the intensity variable (cut at 255cm should never be exceeded?) + - 2 - publishes the echo number as the intensity value (0,1,2,3) + - 3 - publishes the scan layer as the intensity value (0,1,2,3) + - 0 - No intensity data + +...@par Example: + +...@verbatim +driver +( + name "sickLDMRS" + provides ["laser:0"] + requires ["opaque:0"] + buffer_size 20480 + layer 3 +) + +driver +( + name "tcpstream" + provides ["opaque:0"] + port 12002 + ip "10.99.0.1" + buffer_size 10000 +) +...@endverbatim + +...@author Chris Chambers + + +*/ +/** @} */ + +#define DEG2RAD(x) (((double)(x))*0.01745329251994) +//#define RAD2DEG(x) (((double)(x))*57.29577951308232) + +#include <unistd.h> +#include <string.h> +#include <iostream> +#include <arpa/inet.h> +//#include <stdint.h> +//#include <sys/time.h> +//#include <time.h> +//#include <assert.h> +//#include <math.h> + +#include <libplayercore/playercore.h> + +#define LUX_DEFAULT_RX_BUFFER_SIZE 100000U +#define LUX_DEFAULT_LAYER -1 +#define LUX_DEFAULT_ECHO -1 +#define LUX_DEFAULT_INTENSITY 1 + +#define HEADER_LEN 24U +#define SCAN_HEADER_LEN 46U +#define SCAN_DATA_LEN 24U + +#define MESSAGE_LEN 38U + +using namespace std; + +//extern PlayerTime *GlobalTime; + +/////////////////////////////////////////////////////////////////////////////// +// The class for the driver +class sickLDMRS : public ThreadedDriver +{ + public: + // Constructor; need that + sickLDMRS(ConfigFile* cf, int section); + ~sickLDMRS(); + + // Must implement the following methods. + virtual int MainSetup(); + virtual void MainQuit(); + + // This method will be invoked on each incoming message + virtual int ProcessMessage(QueuePointer & resp_queue, + player_msghdr* hdr, + void* data); + + private: + + //Makes a filter command + void makeStartStopCommand(uint8_t* buffer, bool start = true); + + bool ProcessLaserData(); + + // Main function for device thread. + virtual void Main(); + + // Opaque Driver info + Device *opaque; + player_devaddr_t opaque_id; + + player_laser_data_scanangle_t data_packet; + + struct timeval debug_time; + + int layer, echo, intensity; + + // rx buffer + uint8_t * rx_buffer; + unsigned int rx_buffer_size; + unsigned int rx_count; + +}; + + + +//////////////////////////////////////////////////////////////////////////////// +// Now the driver + +// A factory creation function, declared outside of the class so that it +// can be invoked without any object context (alternatively, you can +// declare it static in the class). In this function, we create and return +// (as a generic Driver*) a pointer to a new instance of this driver. +Driver* +sickLDMRS_Init(ConfigFile* cf, int section) +{ + // Create and return a new instance of this driver + return ((Driver*)(new sickLDMRS(cf, section))); + +} + +// A driver registration function, again declared outside of the class so +// that it can be invoked without object context. In this function, we add +// the driver into the given driver table, indicating which interface the +// driver can support and how to create a driver instance. +void sickLDMRS_Register(DriverTable* table) +{ + table->AddDriver("sickLDMRS", sickLDMRS_Init); +} + +//////////////////////////////////////////////////////////////////////////////// +// Constructor. Retrieve options from the configuration file and do any +// pre-Setup() setup. +sickLDMRS::sickLDMRS(ConfigFile* cf, int section) + : ThreadedDriver(cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_LASER_CODE) +{ + this->opaque = NULL; + // Must have an opaque device + if (cf->ReadDeviceAddr(&this->opaque_id, section, "requires", + PLAYER_OPAQUE_CODE, -1, NULL) != 0) + { + puts ("No Opaque driver specified"); + this->SetError(-1); + return; + } + + // Read options from the configuration file + + rx_count = 0; + rx_buffer_size = cf->ReadInt(section, "buffer_size", LUX_DEFAULT_RX_BUFFER_SIZE); + rx_buffer = new uint8_t[rx_buffer_size]; + assert(rx_buffer); + + layer = cf->ReadInt(section, "layer", LUX_DEFAULT_LAYER); + echo = cf->ReadInt(section, "echo", LUX_DEFAULT_ECHO); + intensity = cf->ReadInt(section, "intensity", LUX_DEFAULT_INTENSITY); + + //this->RegisterProperty ("mirror", &this->mirror, cf, section); + + return; +} + +sickLDMRS::~sickLDMRS() +{ + delete [] rx_buffer; +} + +//////////////////////////////////////////////////////////////////////////////// +// Set up the device. Return 0 if things go well, and -1 otherwise. +int sickLDMRS::MainSetup() +{ + puts("Setting up sickLDMRS driver"); + + if(Device::MatchDeviceAddress(this->opaque_id, this->device_addr)) + { + PLAYER_ERROR("attempt to subscribe to self"); + return(-1); + } + + if(!(this->opaque = deviceTable->GetDevice(this->opaque_id))) + { + PLAYER_ERROR("unable to locate suitable opaque device"); + return(-1); + } + + if(this->opaque->Subscribe(this->InQueue) != 0) + { + PLAYER_ERROR("unable to subscribe to opaque device"); + return(-1); + } + + uint8_t* buffer = new uint8_t[MESSAGE_LEN]; + assert(buffer); + makeStartStopCommand(buffer, true); + player_opaque_data_t mData; + mData.data_count = MESSAGE_LEN; + mData.data = buffer; + + opaque->PutMsg(this->InQueue, PLAYER_MSGTYPE_CMD, PLAYER_OPAQUE_CMD_DATA, reinterpret_cast<void*>(&mData),0,NULL); + + delete [] buffer; + buffer = NULL; + + puts("sickLDMRS driver ready"); + + return(0); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Shutdown the device +void sickLDMRS::MainQuit() +{ + uint8_t* buffer = new uint8_t[MESSAGE_LEN]; + assert(buffer); + makeStartStopCommand(buffer, false); + player_opaque_data_t mData; + mData.data_count = MESSAGE_LEN; + mData.data = buffer; + + opaque->PutMsg(this->InQueue, PLAYER_MSGTYPE_CMD, PLAYER_OPAQUE_CMD_DATA, reinterpret_cast<void*>(&mData),0,NULL); + + delete [] buffer; + + puts("sickLDMRS driver shutting down"); + + opaque->Unsubscribe(InQueue); + + puts("sickLDMRS driver has been shutdown"); +} + + + +// Process an incoming message +int sickLDMRS::ProcessMessage(QueuePointer & resp_queue, + player_msghdr* hdr, + void* data) +{ + assert(hdr); + //assert(data); + + if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA, PLAYER_OPAQUE_DATA_STATE, opaque_id)) + { + //GlobalTime->GetTime(&(this->debug_time)); + //PLAYER_MSG2(1,"LUX driver received data packet at %d:%d", this->debug_time.tv_sec, this->debug_time.tv_usec); + //puts("Received tcp data"); + player_opaque_data_t * recv = reinterpret_cast<player_opaque_data_t * > (data); + unsigned int messageOffset = rx_count; + rx_count += recv->data_count; + if (rx_count > rx_buffer_size) + { + PLAYER_WARN("sickLDMRS driver Buffer Full"); + rx_count = 0; + } + else + { + memcpy(&rx_buffer[messageOffset], recv->data, recv->data_count); + } + return 0; + } + return -1; +} + +//////////////////////////////////////////////////////////////////////////////// +// Main function for device thread +void sickLDMRS::Main() +{ + + // The main loop; interact with the device here + for(;;) + { + // Process incoming messages + ProcessMessages(); + + // Ask for the current position and publish it + ProcessLaserData(); + + if (!this->Wait(1)) + { + PLAYER_WARN("No TCP data received within 1s, possible loss of connection"); + } + } + return; +} + +// Currently this alternates between reading the pedal position and reading the steering position +bool sickLDMRS::ProcessLaserData() +{ + //TODO: Implement a timeout here so that if it gets no data back in a time frame it either errors or sends a reset and stuff + while(rx_count > HEADER_LEN) + { + //puts("Got enough data"); + // find our continuous data header + unsigned int ii; + bool found = false; + for (ii = 0; ii < rx_count - HEADER_LEN; ++ii) + { + if (memcmp(&rx_buffer[ii],"\xaf\xfe\xc0\xc2",4) == 0) + { + //PLAYER_MSG0(3,"Header string found"); + memmove(rx_buffer, &rx_buffer[ii], rx_count-ii); + rx_count -= ii; + found = true; + break; + } + } + if (!found) + { + memmove(rx_buffer, &rx_buffer[ii], rx_count-ii); + rx_count -= ii; + return 0; + } + + // get relevant bits of the header + // size includes all data in the data block + // through to the end of the packet including the checksum + uint32_t size = htonl(*reinterpret_cast<uint32_t*> (&rx_buffer[8])); // Should this be htons???? + if (size > rx_buffer_size - HEADER_LEN) + { + PLAYER_WARN("sickLDMRS: Requested Size of data is larger than the buffer size"); + memmove(rx_buffer, &rx_buffer[1], --rx_count); + return 0; + } + + // check if we have enough data yet + if (size > rx_count - HEADER_LEN) + { + return 0; + } + //PLAYER_MSG0(5, "GOT ENOUGH DATATATATATATATATATATATATATATATATATATATATAT"); + + uint8_t * data = &rx_buffer[HEADER_LEN]; + if (memcmp(&rx_buffer[14], "\x22\x01", 2) == 0) + { + //PLAYER_MSG0(1, "Got scan data packet"); + if (size < SCAN_HEADER_LEN) + { + PLAYER_WARN1("sickLDMRS- bad data count (%d)\n", size); + memmove(rx_buffer, &rx_buffer[1], --rx_count); + continue; + } + if (size == SCAN_HEADER_LEN) + { + PLAYER_MSG0(1, "LUX - no scans returned"); + continue; + } + //PLAYER_MSG0(1, "Not bad count"); + //PLAYER_MSG2(1, "Size hex %x %x", rx_buffer[34+HEADER_LEN], rx_buffer[35+HEADER_LEN]); + + uint16_t scan_count = htons(*reinterpret_cast<uint16_t*> (&data[34])); + //uint16_t scan_count = htons(*reinterpret_cast<uint16_t*> (&rx_buffer[34+HEADER_LEN])); + //PLAYER_MSG1(4,"Scan count %d", scan_count); + if (size != (SCAN_HEADER_LEN + SCAN_DATA_LEN*scan_count)) + { + PLAYER_WARN2("sickLDMRS - data size mismatch, size = %d, number of scans = %d\n", size, scan_count); + memmove(rx_buffer, &rx_buffer[1], --rx_count); + continue; + } + + if (intensity) + data_packet.intensity = new uint8_t [scan_count]; + data_packet.ranges = new float [scan_count]; + data_packet.angles = new float [scan_count]; + data_packet.max_range = 300; + // This is needed as the true count will be less than the scan count if you are only interesed in a single layer + // or a single echo thingy + int true_count = 0; + data_packet.id = htons(*reinterpret_cast<uint16_t *>(&data[6])); + for (int ii = 0; ii < scan_count; ++ii) + { + if (!(layer == -1 || layer == data[SCAN_HEADER_LEN+SCAN_DATA_LEN*ii+2])) + continue; + if (!(echo == -1 || echo == data[SCAN_HEADER_LEN+SCAN_DATA_LEN*ii+3])) + continue; + uint16_t Distance_CM = htons(*reinterpret_cast<uint16_t *> (&data[SCAN_HEADER_LEN+SCAN_DATA_LEN*ii+6])); + assert(SCAN_HEADER_LEN + SCAN_DATA_LEN*ii+6+HEADER_LEN <= rx_count); + double distance_m = static_cast<double>(Distance_CM)/100.0; + data_packet.ranges[true_count] = distance_m; + int16_t angle_i = htons(*reinterpret_cast<uint16_t *> (&data[SCAN_HEADER_LEN+SCAN_DATA_LEN*ii+4])); + if (angle_i > 180*32) + { + angle_i -= 360*32; + } + double angle_d = DEG2RAD(angle_i)/32.0; + data_packet.angles[true_count] = angle_d; + if (intensity == 1) + { + // echo width in cm + uint16_t tmp = htons(*reinterpret_cast<uint16_t *> (&data[SCAN_HEADER_LEN+SCAN_DATA_LEN*ii+8])); + if (tmp > 255) + tmp = 255; + data_packet.intensity[true_count] = tmp; + } + else if (intensity == 2) + data_packet.intensity[true_count] = data[SCAN_HEADER_LEN+SCAN_DATA_LEN*ii+3]; + else if (intensity == 3) + data_packet.intensity[true_count] = data[SCAN_HEADER_LEN+SCAN_DATA_LEN*ii+2]; + true_count++; + + } + assert(true_count <= scan_count); + data_packet.ranges_count = true_count; + data_packet.angles_count = true_count; + if (intensity) + data_packet.intensity_count = true_count; + else + data_packet.intensity_count = 0; + // About to publish stuff + //GlobalTime->GetTime(&(this->debug_time)); + //PLAYER_MSG2(1,"LUX driver publishing data at time %d:%d", this->debug_time.tv_sec, this->debug_time.tv_usec); + //PLAYER_MSG0(1, "Publishing data"); + //PLAYER_MSG2(1, "Scan number = %x %x", data[6], data[7]); + //PLAYER_MSG2(1, "Rx_count %d size %d", rx_count, size); + this->Publish (this->device_addr, PLAYER_MSGTYPE_DATA, + PLAYER_LASER_DATA_SCANANGLE, (void*)&data_packet); + delete [] data_packet.ranges; + delete [] data_packet.angles; + delete [] data_packet.intensity; + + + } + else if (memcmp(&rx_buffer[14], "\x20\x20", 2) == 0) + { + if (size != 2) + { + PLAYER_ERROR("Ibeo LUX: Only set mode ack returns are currently supported"); + } + // Got a command message reply + if (data[1] & 0x80) + PLAYER_ERROR("Ibeo LUX: The laser returned a failed flag for the command message sent"); + + } + else + PLAYER_WARN("Ibeo LUX got a unrecognised responce type"); + memmove(rx_buffer, &rx_buffer[size+HEADER_LEN], rx_count - (size+HEADER_LEN)); + rx_count -= (size + HEADER_LEN); + continue; + } + return 1; +} + +void sickLDMRS::makeStartStopCommand(uint8_t* buffer, bool start /*= true*/ ) +{ + + buffer[0] = 0xAF; // magic word + buffer[1] = 0xFE; + buffer[2] = 0xC0; + buffer[3] = 0xC2; + buffer[4] = 0x00; // Size of previous message, here just left to nullL + buffer[5] = 0x00; + buffer[6] = 0x00; + buffer[7] = 0x00; + buffer[8] = 0x00; // Size of data block + buffer[9] = 0x00; + buffer[10] = 0x00; + buffer[11] = 0x0e; + buffer[12] = 0x00; // Reserved + source Id + buffer[13] = 0x00; + buffer[14] = 0x20; // Data Type - 2010 = command + buffer[15] = 0x10; + buffer[16] = 0x00; // 4* ntpp time (s) + 4* fractions of a second + buffer[17] = 0x00; + buffer[18] = 0x00; + buffer[19] = 0x00; + buffer[20] = 0x00; + buffer[21] = 0x00; + buffer[22] = 0x00; + buffer[23] = 0x00; + // Data Block - all little endian + buffer[24] = 0x02; // Command Type - 0002 = set mode + buffer[25] = 0x00; + buffer[26] = 0x00; // Version + buffer[27] = 0x00; + // Start of set mode stuff - all little endian + buffer[28] = 0xc0; // start angle /32 + buffer[29] = 0x26; + buffer[30] = 0x40; // end angle /32 + buffer[31] = 0x06; + buffer[32] = 0x80; // frequency /256, must be 12.5 Hz + buffer[33] = 0x0c; + if (start) + { + buffer[34] = 0x03; + buffer[35] = 0x03; + } + else + { + buffer[34] = 0x00; + buffer[35] = 0x00; + } + buffer[36] = 0x00; // reserved + buffer[37] = 0x00; + + +} + + + + + Modified: code/player/trunk/server/drivers/shell/readlog.cc =================================================================== --- code/player/trunk/server/drivers/shell/readlog.cc 2009-01-09 21:03:45 UTC (rev 7258) +++ code/player/trunk/server/drivers/shell/readlog.cc 2009-01-09 21:14:24 UTC (rev 7259) @@ -1559,6 +1559,55 @@ return ret; } + case PLAYER_LASER_DATA_SCANANGLE: + { + player_laser_data_scanangle_t data; + + if (token_count < 13) + { + PLAYER_ERROR2("incomplete line at %s:%d", + this->filename, linenum); + return -1; + } + + data.id = atoi(tokens[7]); + data.max_range = atof(tokens[8]); + data.ranges_count = atoi(tokens[9]); + data.intensity_count = data.ranges_count; + data.angles_count = data.ranges_count; + + data.ranges = new float[ data.ranges_count ]; + data.intensity = new uint8_t[ data.ranges_count ]; + data.angles = new float[ data.ranges_count ]; + + count = 0; + for (i = 10; i < token_count; i += 3) + { + data.ranges[count] = atof(tokens[i + 0]); + data.angles[count] = atof(tokens[i + 1]); + data.intensity[count] = atoi(tokens[i + 2]); + count += 1; + } + + if (count != (int)data.ranges_count) + { + PLAYER_ERROR2("range count mismatch at %s:%d", + this->filename, linenum); + ret = -1; + } + else + { + this->Publish(id, type, subtype, + (void*)&data, sizeof(data), &time); + } + delete [] data.ranges; + delete [] data.intensity; + delete [] data.angles; + + return ret; + } + + default: PLAYER_ERROR1("unknown laser data subtype %d\n", subtype); return(-1); Modified: code/player/trunk/server/drivers/shell/writelog.cc =================================================================== --- code/player/trunk/server/drivers/shell/writelog.cc 2009-01-09 21:03:45 UTC (rev 7258) +++ code/player/trunk/server/drivers/shell/writelog.cc 2009-01-09 21:14:24 UTC (rev 7259) @@ -983,6 +983,7 @@ player_laser_data_t* scan; player_laser_data_scanpose_t* scanpose; player_laser_geom_t* geom; + player_laser_data_scanangle_t* scanangle; // Check the type switch(hdr->type) @@ -1032,6 +1033,22 @@ } return(0); + case PLAYER_LASER_DATA_SCANANGLE: + scanangle = (player_laser_data_scanangle_t*)data; + fprintf(this->file, "%04d %+07.4f %04d ", + scanangle->id, scanangle->max_range, scanangle->ranges_count); + + for (i = 0; i < scanangle->ranges_count; i++) + { + fprintf(this->file, "%.3f ", scanangle->ranges[i]); + fprintf(this->file, "%.3f ", scanangle->angles[i]); + if(i < scanangle->intensity_count) + fprintf(this->file, "%2d ", scanangle->intensity[i]); + else + fprintf(this->file, "%2d ", 0); + } + return(0); + default: return(-1); } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. ------------------------------------------------------------------------------ Check out the new SourceForge.net Marketplace. It is the best place to buy or sell services for just about anything Open Source. http://p.sf.net/sfu/Xq1LFB _______________________________________________ Playerstage-commit mailing list Playerstage-commit@lists.sourceforge.net https://lists.sourceforge.net/lists/listinfo/playerstage-commit