Update of /cvsroot/playerstage/code/player/server/drivers/laser
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv25284/server/drivers/laser
Modified Files:
sicks3000.cc
Log Message:
added serial stream opaque driver and modified s3000 and nav200 drivers to use
it
Index: sicks3000.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/laser/sicks3000.cc,v
retrieving revision 1.6
retrieving revision 1.7
diff -C2 -d -r1.6 -r1.7
*** sicks3000.cc 1 Nov 2007 22:16:19 -0000 1.6
--- sicks3000.cc 6 Dec 2007 02:35:36 -0000 1.7
***************
*** 49,53 ****
@par Requires
! - none
@par Configuration requests
--- 49,53 ----
@par Requires
! - opaque
@par Configuration requests
***************
*** 58,71 ****
@par Configuration file options
- - port (string)
- - Default: "/dev/ttyS0"
- - Serial port to which laser is attached. If you are using a
- USB/232 or USB/422 converter, this will be "/dev/ttyUSBx".
-
- - transfer_rate (integer)
- - Rate desired for data transfers, negotiated after connection
- - Default: 38400
- - Baud rate. Valid values are 9600, 19200, 38400, 125k, 250k, 500k
-
- pose (length tuple)
- Default: [0.0 0.0 0.0]
--- 58,61 ----
***************
*** 84,89 ****
--- 74,87 ----
name "sicks3000"
provides ["laser:0"]
+ requires ["opaque:0"]
+ )
+
+ driver
+ (
+ name "serialstream"
+ provides ["opaque:0]
port "/dev/ttyS0"
)
+
@endverbatim
***************
*** 113,132 ****
#include <arpa/inet.h> // for htons etc
- #undef HAVE_HI_SPEED_SERIAL
- #ifdef HAVE_LINUX_SERIAL_H
- #ifndef DISABLE_HIGHSPEEDSICK
- #include <linux/serial.h>
- #define HAVE_HI_SPEED_SERIAL
- #endif
- #endif
-
#include <libplayercore/playercore.h>
! #include <replace/replace.h>
extern PlayerTime* GlobalTime;
! #define DEFAULT_LASER_PORT "/dev/ttyS0"
! #define DEFAULT_LASER_TRANSFER_RATE 38400
!
! #define RX_BUFFER_SIZE 4096
// The laser device class.
--- 111,119 ----
#include <arpa/inet.h> // for htons etc
#include <libplayercore/playercore.h>
! //#include <replace/replace.h>
extern PlayerTime* GlobalTime;
! #define DEFAULT_RX_BUFFER_SIZE 4096
// The laser device class.
***************
*** 151,175 ****
virtual void Main();
- // Open the terminal
- // Returns 0 on success
- int OpenTerm();
-
- // Close the terminal
- // Returns 0 on success
- int CloseTerm();
-
- // Set the terminal speed
- // can be any value valid for the s3000
- // Returns 0 on success
- int ChangeTermSpeed(int speed);
-
- // Set the laser data rate
- // Valid values are 9600 and 38400
- // Returns 0 on success
- int SetLaserSpeed(int speed);
-
- // Read range data from laser
- int ReadLaserData();
-
// Process range data from laser
int ProcessLaserData();
--- 138,141 ----
***************
*** 181,184 ****
--- 147,151 ----
// Get the time (in ms)
int64_t GetTime();
+
protected:
***************
*** 188,197 ****
double size[2];
- // Name of device used to communicate with the laser
- const char *device_name;
-
- // laser device file descriptor
- int laser_fd;
-
// Scan width and resolution.
int scan_width, scan_res;
--- 155,158 ----
***************
*** 204,225 ****
// the values used by the laser.
int scan_min_segment, scan_max_segment;
-
- // Range resolution (1 = 1mm, 10 = 1cm, 100 = 10cm).
- int range_res;
-
- // Turn intensity data on/off
- bool intensity;
-
- // Is the laser upside-down? (if so, we'll reverse the ordering of the
- // readings)
- int invert;
-
- bool can_do_hi_speed;
- int connect_rate; // Desired rate for first connection
- int transfer_rate; // Desired rate for operation
- int current_rate; // Current rate
-
- int scan_id;
// rx buffer
uint8_t * rx_buffer;
--- 165,173 ----
// the values used by the laser.
int scan_min_segment, scan_max_segment;
+ // Opaque Driver info
+ Device *opaque;
+ player_devaddr_t opaque_id;
+
// rx buffer
uint8_t * rx_buffer;
***************
*** 230,238 ****
player_laser_data_t data_packet;
player_laser_config_t config_packet;
-
-
- #ifdef HAVE_HI_SPEED_SERIAL
- struct serial_struct old_serial;
- #endif
};
--- 178,181 ----
***************
*** 267,285 ****
: Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN,
PLAYER_LASER_CODE)
{
// allocate our recieve buffer
! rx_buffer_size = RX_BUFFER_SIZE;
rx_buffer = new uint8_t[rx_buffer_size];
assert(rx_buffer);
memset(&data_packet,0,sizeof(data_packet));
! data_packet.min_angle = DTOR(-95);
! data_packet.max_angle = DTOR(95);
! data_packet.resolution = DTOR(0.25);
data_packet.max_range = 49;
memset(&config_packet,0,sizeof(config_packet));
! config_packet.min_angle = DTOR(-95);
! config_packet.max_angle = DTOR(95);
! config_packet.resolution = DTOR(0.25);
config_packet.max_range = 49;
--- 210,230 ----
: Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN,
PLAYER_LASER_CODE)
{
+
+ rx_count = 0;
// allocate our recieve buffer
! rx_buffer_size = cf->ReadInt(section, "buffer_size",
DEFAULT_RX_BUFFER_SIZE);
rx_buffer = new uint8_t[rx_buffer_size];
assert(rx_buffer);
memset(&data_packet,0,sizeof(data_packet));
! data_packet.min_angle = DTOR(-135);
! data_packet.max_angle = DTOR(135);
! data_packet.resolution = DTOR(0.5);
data_packet.max_range = 49;
memset(&config_packet,0,sizeof(config_packet));
! config_packet.min_angle = DTOR(-135);
! config_packet.max_angle = DTOR(135);
! config_packet.resolution = DTOR(0.5);
config_packet.max_range = 49;
***************
*** 291,314 ****
this->size[1] = 0.15;
! // Serial port
! this->device_name = cf->ReadString(section, "port", DEFAULT_LASER_PORT);
!
! // Serial rate
! this->transfer_rate = cf->ReadInt(section, "transfer_rate",
DEFAULT_LASER_TRANSFER_RATE);
! this->current_rate = 0;
!
! #ifdef HAVE_HI_SPEED_SERIAL
! this->can_do_hi_speed = true;
! #else
! this->can_do_hi_speed = false;
! #endif
!
! if (!this->can_do_hi_speed && this->transfer_rate > 38400)
{
! PLAYER_ERROR1("sicklms200: requested hi speed serial, but no support
compiled in. Defaulting to %d bps.",
! DEFAULT_LASER_TRANSFER_RATE);
! this->connect_rate = DEFAULT_LASER_TRANSFER_RATE;
}
return;
}
--- 236,250 ----
this->size[1] = 0.15;
! 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;
}
+
return;
}
***************
*** 324,336 ****
int SickS3000::Setup()
{
! PLAYER_MSG1(2, "Laser initialising (%s)", this->device_name);
!
! // Open the terminal
! if (OpenTerm())
! return 1;
!
! if (ChangeTermSpeed(this->transfer_rate))
{
! return 1;
}
--- 260,282 ----
int SickS3000::Setup()
{
!
! PLAYER_MSG0(2, "Laser initialising");
! // Subscribe to the opaque device.
! 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);
}
***************
*** 339,343 ****
// Start the device thread
StartThread();
!
return 0;
}
--- 285,289 ----
// Start the device thread
StartThread();
!
return 0;
}
***************
*** 351,356 ****
StopThread();
! CloseTerm();
!
PLAYER_MSG0(2, "laser shutdown");
--- 297,302 ----
StopThread();
! opaque->Unsubscribe(InQueue);
!
PLAYER_MSG0(2, "laser shutdown");
***************
*** 364,367 ****
--- 310,323 ----
void * data)
{
+
+ if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
PLAYER_OPAQUE_DATA_STATE, opaque_id))
+ {
+ player_opaque_data_t * recv = reinterpret_cast<player_opaque_data_t * >
(data);
+ memmove(&rx_buffer[rx_count], recv->data, recv->data_count);
+ rx_count += recv->data_count;
+ ProcessLaserData();
+ return 0;
+ }
+
if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_LASER_REQ_GET_CONFIG,
***************
*** 394,398 ****
return(0);
}
-
// Don't know how to handle this message.
return(-1);
--- 350,353 ----
***************
*** 405,408 ****
--- 360,366 ----
for(;;)
{
+ // Waits for the opaque driver to pass data onto this thread.
+ InQueue->Wait();
+
// test if we are supposed to cancel
pthread_testcancel();
***************
*** 411,618 ****
ProcessMessages();
! // read data into our ring buffer and then process it
! int ret = ReadLaserData();
! if (ret < 0)
! {
! PLAYER_WARN("Error reading from S3000 device");
! }
! else if (ret > 0)
! ProcessLaserData();
! }
! }
!
!
!
!
////////////////////////////////////////////////////////////////////////////////
! // Open the terminal
! // Returns 0 on success
! int SickS3000::OpenTerm()
! {
! this->laser_fd = ::open(this->device_name, O_RDWR | O_SYNC , S_IRUSR |
S_IWUSR );
! if (this->laser_fd < 0)
! {
! PLAYER_ERROR2("unable to open serial port [%s]; [%s]",
! (char*) this->device_name, strerror(errno));
! return 1;
! }
!
! // set the serial port speed to 9600 to match the laser
! // later we can ramp the speed up to the SICK's 38K
! //
! struct termios term;
! if( tcgetattr( this->laser_fd, &term ) < 0 )
! RETURN_ERROR(1, "Unable to get serial port attributes");
!
! cfmakeraw( &term );
! cfsetispeed( &term, B9600 );
! cfsetospeed( &term, B9600 );
!
! if( tcsetattr( this->laser_fd, TCSAFLUSH, &term ) < 0 )
! RETURN_ERROR(1, "Unable to set serial port attributes");
!
! // Make sure queue is empty
! //
! tcflush(this->laser_fd, TCIOFLUSH);
!
! return 0;
! }
!
!
!
////////////////////////////////////////////////////////////////////////////////
! // Close the terminal
! // Returns 0 on success
! //
! int SickS3000::CloseTerm()
! {
! ::close(this->laser_fd);
! return 0;
! }
!
!
!
////////////////////////////////////////////////////////////////////////////////
! // Set the terminal speed
! // Valid values are 9600 and 38400
! // Returns 0 on success
! //
! int SickS3000::ChangeTermSpeed(int speed)
! {
! struct termios term;
!
! current_rate = speed;
!
! #ifdef HAVE_HI_SPEED_SERIAL
! struct serial_struct serial;
!
! // we should check and reset the AYSNC_SPD_CUST flag
! // since if it's set and we request 38400, we're likely
! // to get another baud rate instead (based on custom_divisor)
! // this way even if the previous player doesn't reset the
! // port correctly, we'll end up with the right speed we want
! if (ioctl(this->laser_fd, TIOCGSERIAL, &serial) < 0)
! {
! //RETURN_ERROR(1, "error on TIOCGSERIAL in beginning");
! PLAYER_WARN("ioctl() failed while trying to get serial port info");
! }
! else
! {
! serial.flags &= ~ASYNC_SPD_CUST;
! serial.custom_divisor = 0;
! if (ioctl(this->laser_fd, TIOCSSERIAL, &serial) < 0)
! {
! //RETURN_ERROR(1, "error on TIOCSSERIAL in beginning");
! PLAYER_WARN("ioctl() failed while trying to set serial port info");
! }
! }
! #endif
!
! //printf("LASER: change TERM speed: %d\n", speed);
!
! int term_speed;
! switch(speed)
! {
! case 9600:
! term_speed = B9600;
! break;
! case 19200:
! term_speed = B19200;
! break;
! case 38400:
! term_speed = B38400;
! break;
! default:
! term_speed = speed;
! }
!
! switch(term_speed)
! {
! case B9600:
! case B19200:
! case B38400:
! //PLAYER_MSG0(2, "terminal speed to 9600");
! if( tcgetattr( this->laser_fd, &term ) < 0 )
! RETURN_ERROR(1, "unable to get device attributes");
!
! cfmakeraw( &term );
! cfsetispeed( &term, term_speed );
! cfsetospeed( &term, term_speed );
!
! if( tcsetattr( this->laser_fd, TCSAFLUSH, &term ) < 0 )
! RETURN_ERROR(1, "unable to set device attributes");
! break;
!
! case 500000:
! //PLAYER_MSG0(2, "terminal speed to 500000");
!
! #ifdef HAVE_HI_SPEED_SERIAL
! if (ioctl(this->laser_fd, TIOCGSERIAL, &this->old_serial) < 0) {
! RETURN_ERROR(1, "error on TIOCGSERIAL ioctl");
! }
!
! serial = this->old_serial;
!
! serial.flags |= ASYNC_SPD_CUST;
! serial.custom_divisor = 48; // for FTDI USB/serial converter divisor is
240/5
!
! if (ioctl(this->laser_fd, TIOCSSERIAL, &serial) < 0) {
! RETURN_ERROR(1, "error on TIOCSSERIAL ioctl");
! }
!
! #else
! fprintf(stderr, "sicklms200: Trying to change to 500kbps, but no
support compiled in, defaulting to 38.4kbps.\n");
! #endif
!
! // even if we are doing 500kbps, we have to set the speed to 38400...
! // the driver will know we want 500000 instead.
!
! if( tcgetattr( this->laser_fd, &term ) < 0 )
! RETURN_ERROR(1, "unable to get device attributes");
!
! cfmakeraw( &term );
! cfsetispeed( &term, B38400 );
! cfsetospeed( &term, B38400 );
!
! if( tcsetattr( this->laser_fd, TCSAFLUSH, &term ) < 0 )
! RETURN_ERROR(1, "unable to set device attributes");
- break;
- default:
- PLAYER_ERROR1("unknown speed %d", speed);
- }
- return 0;
- }
-
-
-
-
-
-
-
////////////////////////////////////////////////////////////////////////////////
- // Read range data from laser
- //
- int SickS3000::ReadLaserData()
- {
- if (rx_count == rx_buffer_size)
- {
- PLAYER_WARN("S3000 RX buffer full\n");
- return 0;
- }
- // Read a packet from the laser
- //
- //int len = ReadFromLaser(&rx_buffer[rx_count], rx_buffer_size - rx_count);
- int len = read(this->laser_fd, &rx_buffer[rx_count], rx_buffer_size -
rx_count);
- if (len == 0)
- {
- PLAYER_MSG0(2, "empty packet");
- return 0;
- }
- if (len < 0)
- {
- PLAYER_ERROR2("error reading form s3000: %d %s", errno, strerror(errno));
- return -1;
}
-
- rx_count += len;
-
- return len;
}
--- 369,375 ----
ProcessMessages();
! //usleep(1000);
}
}
***************
*** 622,625 ****
--- 379,383 ----
while(rx_count >= 22)
{
+
// find our continuous data header
unsigned int ii;
***************
*** 646,649 ****
--- 404,414 ----
// through to the end of the packet including the checksum
unsigned short size = 2*htons(*reinterpret_cast<unsigned short *>
(&rx_buffer[6]));
+ //printf("size %d", size);
+ if (size > rx_buffer_size - 26)
+ {
+ PLAYER_WARN("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
***************
*** 703,707 ****
}
! memmove(rx_buffer, &rx_buffer[size+4], size+4);
rx_count -= (size + 4);
continue;
--- 468,472 ----
}
! memmove(rx_buffer, &rx_buffer[size+4], rx_count - (size+4));
rx_count -= (size + 4);
continue;
***************
*** 756,791 ****
return CRC_16;
}
!
!
!
! /*
!
////////////////////////////////////////////////////////////////////////////////
! // Create a CRC for the given packet
! //
! unsigned short SickS3000::CreateCRC(uint8_t* data, ssize_t len)
! {
! uint16_t uCrc16;
! uint8_t abData[2];
!
! uCrc16 = 0;
! abData[0] = 0;
!
! while(len-- )
! {
! abData[1] = abData[0];
! abData[0] = *data++;
!
! if( uCrc16 & 0x8000 )
! {
! uCrc16 = (uCrc16 & 0x7fff) << 1;
! uCrc16 ^= CRC16_GEN_POL;
! }
! else
! {
! uCrc16 <<= 1;
! }
! uCrc16 ^= MAKEUINT16(abData[0],abData[1]);
! }
! return (uCrc16);
! }
! */
--- 521,524 ----
return CRC_16;
}
!
!
-------------------------------------------------------------------------
SF.Net email is sponsored by: The Future of Linux Business White Paper
from Novell. From the desktop to the data center, Linux is going
mainstream. Let it simplify your IT future.
http://altfarm.mediaplex.com/ad/ck/8857-50307-18918-4
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
