Update of /cvsroot/playerstage/code/player/server/drivers/position/nav200
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv25284/server/drivers/position/nav200
Modified Files:
nav200.cc nav200.h sicknav200.cc test.cpp test_program.cpp
test_program_init.cpp
Log Message:
added serial stream opaque driver and modified s3000 and nav200 drivers to use
it
Index: nav200.h
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/position/nav200/nav200.h,v
retrieving revision 1.2
retrieving revision 1.3
diff -C2 -d -r1.2 -r1.3
*** nav200.h 25 Sep 2006 15:53:56 -0000 1.2
--- nav200.h 6 Dec 2007 02:35:36 -0000 1.3
***************
*** 26,32 ****
//#include <stdint.h>
- #define DEFAULT_PORT "/dev/ttyS0"
- #define DEFAULT_RATE B19200
-
#define STX 0x02
--- 26,29 ----
***************
*** 91,98 ****
public:
Nav200();
~Nav200();
! int Initialise(const char * port = DEFAULT_PORT, int rate = DEFAULT_RATE);
int Terminate();
--- 88,96 ----
public:
+ friend class SickNAV200;
Nav200();
~Nav200();
! int Initialise(Driver* device, Device* opaque, player_devaddr_t opaque_id);
int Terminate();
***************
*** 156,159 ****
--- 154,165 ----
int WriteCommand(char mode, char function, int dataLength, uint8_t * data);
uint8_t CreateCRC(uint8_t* data, ssize_t len);
+
+ // SickNav200 Driver info
+ Driver *sn200;
+
+ // Opaque info - for setting filter
+ Device *opaque;
+ player_devaddr_t opaque_id;
+
};
Index: test_program_init.cpp
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/position/nav200/test_program_init.cpp,v
retrieving revision 1.2
retrieving revision 1.3
diff -C2 -d -r1.2 -r1.3
*** test_program_init.cpp 2 Dec 2007 09:11:12 -0000 1.2
--- test_program_init.cpp 6 Dec 2007 02:35:36 -0000 1.3
***************
*** 1,3 ****
! #include <stdlib.h>
#include <time.h>
#include "nav200.h"
--- 1,3 ----
! /*#include <stdlib.h>
#include <time.h>
#include "nav200.h"
***************
*** 103,104 ****
--- 103,108 ----
}
+ */
+
+ int main()
+ {}
\ No newline at end of file
Index: nav200.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/position/nav200/nav200.cc,v
retrieving revision 1.2
retrieving revision 1.3
diff -C2 -d -r1.2 -r1.3
*** nav200.cc 14 Mar 2007 23:19:28 -0000 1.2
--- nav200.cc 6 Dec 2007 02:35:36 -0000 1.3
***************
*** 11,54 ****
}
! int Nav200::Initialise(const char * port, int rate)
{
bytesReceived = 0;
- fd = -1;
-
- // open the serial port
- fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
- if ( fd<0 )
- {
- fprintf(stderr, "Could not open serial device %s\n",port);
- return -1;
- }
-
- // save the current io settings
- tcgetattr(fd, &oldtio);
-
- // set up new settings
- struct termios newtio;
- memset(&newtio, 0,sizeof(newtio));
- newtio.c_cflag = CS8 | CREAD | PARENB;
- newtio.c_iflag = INPCK;
- newtio.c_oflag = 0;
- newtio.c_lflag = 0;
-
- // activate new settings
- tcflush(fd, TCIFLUSH);
- if (cfsetispeed(&newtio, rate) < 0 || cfsetospeed(&newtio, rate) < 0)
- {
- fprintf(stderr,"Failed to set serial baud rate: %d\n", rate);
- tcsetattr(fd, TCSANOW, &oldtio);
- close(fd);
- fd = -1;
- return -1;
- }
- tcsetattr(fd, TCSANOW, &newtio);
- tcflush(fd, TCIOFLUSH);
- // clear the input buffer in case junk data is on the port
- usleep(10000);
- tcflush(fd, TCIFLUSH);
return 0;
}
--- 11,21 ----
}
! int Nav200::Initialise(Driver* sn2002, Device* opaque2, player_devaddr_t
opaque_id2 )
{
+ this->sn200 = sn2002;
+ this->opaque_id = opaque_id2;
+ this->opaque = opaque2;
bytesReceived = 0;
return 0;
}
***************
*** 56,65 ****
int Nav200::Terminate()
{
- // restore old port settings
- if (fd > 0)
- {
- tcsetattr(fd, TCSANOW, &oldtio);
- close(fd);
- }
return 0;
}
--- 23,26 ----
***************
*** 68,71 ****
--- 29,33 ----
* Standby mode
*******************/
+
bool Nav200::EnterStandby()
{
***************
*** 589,594 ****
int Nav200::WriteCommand(char mode, char function, int dataLength, uint8_t *
data)
{
! if (fd < 0)
! return -1;
int length = dataLength+5;
--- 551,556 ----
int Nav200::WriteCommand(char mode, char function, int dataLength, uint8_t *
data)
{
! /* if (fd < 0)
! return -1;*/
int length = dataLength+5;
***************
*** 607,614 ****
buffer[length-1] = CreateCRC(buffer, 4 + dataLength);
! // Make sure both input and output queues are empty
! tcflush(fd, TCIOFLUSH);
! // switch to blocking IO for the write
int flags = fcntl(fd, F_GETFL);
if (flags < 0 || fcntl(fd,F_SETFL,flags &~O_NONBLOCK) < 0)
--- 569,576 ----
buffer[length-1] = CreateCRC(buffer, 4 + dataLength);
! /* // Make sure both input and output queues are empty
! tcflush(fd, TCIOFLUSH);*/
! /* // switch to blocking IO for the write
int flags = fcntl(fd, F_GETFL);
if (flags < 0 || fcntl(fd,F_SETFL,flags &~O_NONBLOCK) < 0)
***************
*** 620,625 ****
return -1;
}
!
! if((length && (write(fd, buffer, length)) < length))
{
fprintf(stderr,"Error writing to FOB (%d - %s),
disabling\n",errno,strerror(errno));
--- 582,592 ----
return -1;
}
! */
! player_opaque_data_t mData;
! mData.data_count = length;
! mData.data = buffer;
! opaque->PutMsg(sn200->InQueue, PLAYER_MSGTYPE_CMD, PLAYER_OPAQUE_CMD_DATA,
reinterpret_cast<void*>(&mData), 0, NULL);
!
! /* if((length && (write(fd, buffer, length)) < length))
{
fprintf(stderr,"Error writing to FOB (%d - %s),
disabling\n",errno,strerror(errno));
***************
*** 628,634 ****
delete [] buffer;
return -1;
! }
! // restore flags
if (fcntl(fd,F_SETFL,flags) < 0)
{
--- 595,601 ----
delete [] buffer;
return -1;
! }*/
! /* // restore flags
if (fcntl(fd,F_SETFL,flags) < 0)
{
***************
*** 638,642 ****
delete [] buffer;
return -1;
! }
delete [] buffer;
--- 605,609 ----
delete [] buffer;
return -1;
! }*/
delete [] buffer;
***************
*** 650,654 ****
int Nav200::ReadFromNav200(int timeout_usec)
{
! int ret;
int dataLength = 0;
--- 617,621 ----
int Nav200::ReadFromNav200(int timeout_usec)
{
!
int dataLength = 0;
***************
*** 656,659 ****
--- 623,628 ----
gettimeofday(&start,NULL);
+ sn200->InQueue->SetFilter(opaque_id.host, opaque_id.robot,
opaque_id.interf, opaque_id.index, PLAYER_MSGTYPE_DATA,
PLAYER_OPAQUE_DATA_STATE);
+
for (;;)
{
***************
*** 664,688 ****
return -1;
}
!
! ret = read(fd, &receivedBuffer[bytesReceived], BUFFER_SIZE -
bytesReceived);
! if (ret < 0)
! {
! fprintf(stderr,"Got error while reading %d %s\n",errno,
strerror(errno));
! return ret;
! }
!
! bytesReceived += ret;
!
! if (ret == 0)
! {
! usleep(1000);
! continue;
! }
!
// do we have enough for a header?
while (bytesReceived > 4)
{
if (receivedBuffer[0] != STX)
! {
// bad thing, we dont have the correct start to a message
for(int i=1; i<bytesReceived; i++)
--- 633,646 ----
return -1;
}
! //puts("waiting for data");
! sn200->ProcessMessages();
!
// do we have enough for a header?
while (bytesReceived > 4)
{
+ //PLAYER_MSG4(2, "recieved STX %d data len %d mode %c fun %c",
receivedBuffer[0], receivedBuffer[1], receivedBuffer[2], receivedBuffer[3]);
if (receivedBuffer[0] != STX)
! {
! bool found = false;
// bad thing, we dont have the correct start to a message
for(int i=1; i<bytesReceived; i++)
***************
*** 692,697 ****
--- 650,662 ----
memmove(receivedBuffer, receivedBuffer+i, bytesReceived-i);
bytesReceived-=i;
+ found = true;
+ break;
}
}
+ // If none of the data points are STX then all are (essentially)
removed
+ if (!found)
+ {
+ bytesReceived = 0;
+ }
continue;
}
***************
*** 711,714 ****
--- 676,680 ----
{// bad thing, we dont have the correct start to a message
fprintf(stderr,"bad CRC!!!\n");
+ bool found = false;
for(int i=1; i<bytesReceived; i++)
{ // find where STX is
***************
*** 717,722 ****
--- 683,695 ----
memmove(receivedBuffer, receivedBuffer+i, bytesReceived-i);
bytesReceived-=i;
+ found = true;
+ break;
}
}
+ // If none of the data points are STX then all are (essentially)
removed
+ if (!found)
+ {
+ bytesReceived = 0;
+ }
continue;
}
***************
*** 732,736 ****
//check out what the error is and it out
PrintErrorMsg();
!
return -2;
}
--- 705,709 ----
//check out what the error is and it out
PrintErrorMsg();
! sn200->InQueue->ClearFilter();
return -2;
}
***************
*** 746,753 ****
--- 719,728 ----
memmove(receivedBuffer, receivedBuffer+dataLength,
bytesReceived-dataLength);
bytesReceived-=dataLength;
+ sn200->InQueue->ClearFilter();
return 1;
}
}
}
+ sn200->InQueue->ClearFilter();
return 0;
}
Index: sicknav200.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/position/nav200/sicknav200.cc,v
retrieving revision 1.8
retrieving revision 1.9
diff -C2 -d -r1.8 -r1.9
*** sicknav200.cc 21 Sep 2007 03:31:50 -0000 1.8
--- sicknav200.cc 6 Dec 2007 02:35:36 -0000 1.9
***************
*** 48,52 ****
@par Requires
! - none
@par Configuration requests
--- 48,52 ----
@par Requires
! - @ref opaque
@par Configuration requests
***************
*** 56,64 ****
@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".
-
- pose (length tuple)
- Default: [0.0 0.0 0.0]
--- 56,59 ----
***************
*** 77,82 ****
name "sicknav200"
provides ["position2d:0"]
! port "/dev/ttyS0"
)
@endverbatim
--- 72,84 ----
name "sicknav200"
provides ["position2d:0"]
! requires ["opaque:0"]
)
+ driver
+ (
+ name "serialstream"
+ provides ["opaque:0"]
+ port "/dev/ttyS0"
+ )
+
@endverbatim
***************
*** 107,111 ****
#include <libplayercore/playercore.h>
! #include <replace/replace.h>
extern PlayerTime* GlobalTime;
--- 109,113 ----
#include <libplayercore/playercore.h>
! // #include <replace/replace.h>
extern PlayerTime* GlobalTime;
***************
*** 140,144 ****
// Name of device used to communicate with the laser
! char *device_name;
// storage for outgoing data
--- 142,148 ----
// Name of device used to communicate with the laser
! //char *device_name;
!
! bool nchanged;
// storage for outgoing data
***************
*** 149,152 ****
--- 153,159 ----
int min_radius, max_radius;
+ // Opaque Driver info
+ Device *opaque;
+ player_devaddr_t opaque_id;
};
***************
*** 161,164 ****
--- 168,172 ----
void SickNAV200_Register(DriverTable* table)
{
+ puts("Registering driver");
table->AddDriver("sicknav200", SickNAV200_Init);
}
***************
*** 181,191 ****
this->size[1] = 0.15;
! // Serial port
! this->device_name = strdup(cf->ReadString(section, "port", DEFAULT_PORT));
// nav200 parameters, convert to cm
this->min_radius = static_cast<int> (cf->ReadLength(section, "min_radius",
1) * 100);
this->max_radius = static_cast<int> (cf->ReadLength(section, "max_radius",
30) * 100);
!
return;
}
--- 189,213 ----
this->size[1] = 0.15;
! nchanged = true;
!
! // Serial port - done in the opaque driver
! //this->device_name = strdup(cf->ReadString(section, "port", DEFAULT_PORT));
// nav200 parameters, convert to cm
this->min_radius = static_cast<int> (cf->ReadLength(section, "min_radius",
1) * 100);
this->max_radius = static_cast<int> (cf->ReadLength(section, "max_radius",
30) * 100);
!
! this->opaque = NULL;
! // Must have an opaque device
! PLAYER_MSG0(2, "reading opaque id now");
! if (cf->ReadDeviceAddr(&this->opaque_id, section, "requires",
! PLAYER_OPAQUE_CODE, -1, NULL) != 0)
! {
! PLAYER_MSG0(2, "No opaque driver specified");
! this->SetError(-1);
! return;
! }
! PLAYER_MSG0(2, "reading opaque id now");
!
return;
}
***************
*** 193,197 ****
SickNAV200::~SickNAV200()
{
! free(device_name);
}
--- 215,219 ----
SickNAV200::~SickNAV200()
{
! //free(device_name);
}
***************
*** 201,218 ****
int SickNAV200::Setup()
{
! PLAYER_MSG1(2, "NAV200 initialising (%s)", this->device_name);
!
! // Open the terminal
! Laser.Initialise(this->device_name);
! if (!Laser.EnterStandby() || !Laser.EnterPositioning())
{
! PLAYER_ERROR("unable to enter standby or position mode\n");
! return -1;;
}
! if (!Laser.SetActionRadii(min_radius, max_radius))
{
! PLAYER_ERROR("failed to set action radii\n");
! return -1;;
}
PLAYER_MSG0(2, "NAV200 ready");
--- 223,251 ----
int SickNAV200::Setup()
{
! PLAYER_MSG0(2, "NAV200 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);
+ }
+
+ // Open the terminal
+ Laser.Initialise(this, opaque, opaque_id);
+ puts("Laser initilised");
+
PLAYER_MSG0(2, "NAV200 ready");
***************
*** 225,228 ****
--- 258,262 ----
+
////////////////////////////////////////////////////////////////////////////////
// Shutdown the device
***************
*** 232,235 ****
--- 266,271 ----
StopThread();
+ opaque->Unsubscribe(InQueue);
+
PLAYER_MSG0(2, "laser shutdown");
***************
*** 243,246 ****
--- 279,290 ----
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(&Laser.receivedBuffer[Laser.bytesReceived], recv->data,
recv->data_count);
+ Laser.bytesReceived += recv->data_count;
+ return 0;
+ }
+
if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_POSITION2D_REQ_GET_GEOM,
***************
*** 279,282 ****
--- 323,340 ----
ProcessMessages();
+ if (nchanged)
+ {
+ if (!Laser.EnterStandby() || !Laser.EnterPositioning())
+ {
+ PLAYER_ERROR("unable to enter standby or position mode\n");
+ return ;
+ }
+ if (!Laser.SetActionRadii(min_radius, max_radius))
+ {
+ PLAYER_ERROR("failed to set action radii\n");
+ return ;
+ }
+ nchanged = false;
+ }
// get update and publish result
if(Laser.GetPositionAuto(Reading))
***************
*** 309,311 ****
-
--- 367,368 ----
Index: test_program.cpp
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/position/nav200/test_program.cpp,v
retrieving revision 1.3
retrieving revision 1.4
diff -C2 -d -r1.3 -r1.4
*** test_program.cpp 1 Dec 2006 02:47:52 -0000 1.3
--- test_program.cpp 6 Dec 2007 02:35:36 -0000 1.4
***************
*** 1,3 ****
! #include <stdlib.h>
#include "nav200.h"
--- 1,3 ----
! /*#include <stdlib.h>
#include "nav200.h"
***************
*** 36,37 ****
--- 36,41 ----
return 0;
}
+ */
+
+ int main()
+ {}
\ No newline at end of file
Index: test.cpp
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/position/nav200/test.cpp,v
retrieving revision 1.2
retrieving revision 1.3
diff -C2 -d -r1.2 -r1.3
*** test.cpp 18 Dec 2006 12:17:22 -0000 1.2
--- test.cpp 6 Dec 2007 02:35:36 -0000 1.3
***************
*** 1,3 ****
! #include <stdlib.h>
#include <time.h>
#include "nav200.h"
--- 1,3 ----
! /*#include <stdlib.h>
#include <time.h>
#include "nav200.h"
***************
*** 9,13 ****
PositionXY reflector;
Nav200 testing;
! testing.Initialise("/dev/ttyS1");
if (testing.EnterStandby())
{
--- 9,13 ----
PositionXY reflector;
Nav200 testing;
! testing.Initialise("/dev/ttyS0");
if (testing.EnterStandby())
{
***************
*** 220,221 ****
--- 220,225 ----
// bool rotateDirection(uint8_t direction); //absolutely not working for
some unknown reason
// bool DeleteReflectorPosition(uint8_t layer, uint8_t number, PositionXY &
reflector); <-- return incorrect X value
+ */
+
+ int main()
+ {}
\ No newline at end of file
-------------------------------------------------------------------------
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
