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
