void
hokuyo::Laser::close ()
{
int retval = 0;
if (portOpen()) {
//Try to be a good citizen and completely shut down the laser before we shutdown communication
try
{
reset();
}
catch (hokuyo::Exception& e) {
//Exceptions here can be safely ignored since we are closing the port anyways
}
#if HOKUYO_INTERFACE_ETHERNET
tcpclient_close(&tcpclient);
#else
retval = ::close(laser_fd_); // Automatically releases the lock.
#endif
}
laser_fd_ = -1;
if (retval != 0)
HOKUYO_EXCEPT(hokuyo::Exception, "Failed to close port properly -- error = %d: %s\n", errno, strerror(errno));
}
void
hokuyo::Laser::open(const char * tcphost, const int tcpport)
{
if (portOpen())
close();
// Make IO non blocking. This way there are no race conditions that
// cause blocking when a badly behaving process does a read at the same
// time as us. Will need to switch to blocking for writes or errors
// occur just after a replug event.
laser_fd_ = tcpclient_open(&tcpclient,tcphost, tcpport);//::open(port_name, O_RDWR | O_NONBLOCK | O_NOCTTY);
//laser_fd_ = tcpclient_open(&tcpclient,"127.0.0.1", 10001);//::open(port_name, O_RDWR | O_NONBLOCK | O_NOCTTY);
read_buf_start = read_buf_end = 0;
if (laser_fd_ < 0)
{
HOKUYO_EXCEPT(hokuyo::Exception, "Failed to open tcp_client: %s:%d. %s (errno = %d)", tcphost,tcpport, strerror(errno), errno);
}
laser_fd_ = tcpclient.sock_desc;
try
{
// Some models (04LX) need to be told to go into SCIP2 mode...
laserFlush();
// Just in case a previous failure mode has left our Hokuyo
// spewing data, we send reset the laser to be safe.
try {
reset();
}
catch (hokuyo::Exception &e)
{
// This might be a device that needs to be explicitely placed in
// SCIP2 mode.
// Note: Not tested: a device that is currently scanning in SCIP1.1
// mode might not manage to switch to SCIP2.0.
setToSCIP2(); // If this fails then it wasn't a device that could be switched to SCIP2.
reset(); // If this one fails, it really is an error.
}
querySensorConfig();
queryVersionInformation(); // In preparation for calls to get various parts of the version info.
}
catch (hokuyo::Exception& e)
{
// These exceptions mean something failed on open and we should close
if (laser_fd_ != -1)
tcpclient_close(&tcpclient);
laser_fd_ = -1;
throw e;
}
}
int tcpclient_open(urg_tcpclient_t* cli, const char* ip_str, int port_num)
{
enum { Connect_timeout_second = 2 };
fd_set rmask, wmask;
struct timeval tv = { Connect_timeout_second, 0 };
#if defined(URG_WINDOWS_OS)
u_long flag;
#else
int flag;
int sock_optval = -1;
int sock_optval_size = sizeof(sock_optval);
#endif
int ret;
cli->sock_desc = Invalid_desc;
cli->pushed_back = -1; // no pushed back char.
#if defined(URG_WINDOWS_OS)
{
static int is_initialized = 0;
WORD wVersionRequested = 0x0202;
WSADATA WSAData;
int err;
if (!is_initialized) {
err = WSAStartup(wVersionRequested, &WSAData);
if (err != 0) {
return -1;
}
is_initialized = 1;
}
}
#endif
tcpclient_buffer_init(cli);
cli->sock_addr_size = sizeof (struct sockaddr_in);
if ((cli->sock_desc = (int)socket(AF_INET, SOCK_STREAM, 0)) < 0) {
return -1;
}
memset((char*)&(cli->server_addr), 0, sizeof(cli->sock_addr_size));
cli->server_addr.sin_family = AF_INET;
cli->server_addr.sin_port = htons(port_num);
if (!strcmp(ip_str, "localhost")) {
ip_str = "127.0.0.1";
}
/* bind is not required, and port number is dynamic */
if ((cli->server_addr.sin_addr.s_addr = inet_addr(ip_str)) == INADDR_NONE) {
return -1;
}
#if defined(URG_WINDOWS_OS)
// Configures non-blocking mode
flag = 1;
ioctlsocket(cli->sock_desc, FIONBIO, &flag);
if (connect(cli->sock_desc, (const struct sockaddr *)&(cli->server_addr),
cli->sock_addr_size) == SOCKET_ERROR) {
int error_number = WSAGetLastError();
if (error_number != WSAEWOULDBLOCK) {
tcpclient_close(cli);
return -1;
}
FD_ZERO(&rmask);
FD_SET((SOCKET)cli->sock_desc, &rmask);
wmask = rmask;
ret = select((int)cli->sock_desc + 1, &rmask, &wmask, NULL, &tv);
if (ret == 0) {
// Operation timed out
tcpclient_close(cli);
return -2;
}
}
// Returns to blocking mode
set_block_mode(cli);
#else
// Configures non-blocking mode
flag = fcntl(cli->sock_desc, F_GETFL, 0);
fcntl(cli->sock_desc, F_SETFL, flag | O_NONBLOCK);
if (connect(cli->sock_desc, (const struct sockaddr *)&(cli->server_addr),
cli->sock_addr_size) < 0) {
if (errno != EINPROGRESS) {
tcpclient_close(cli);
return -1;
}
// EINPROGRESS: a connection request was already received and not completed yet
FD_ZERO(&rmask);
FD_SET(cli->sock_desc, &rmask);
wmask = rmask;
ret = select(cli->sock_desc + 1, &rmask, &wmask, NULL, &tv);
if (ret <= 0) {
// Operation timed out
tcpclient_close(cli);
return -2;
}
if (getsockopt(cli->sock_desc, SOL_SOCKET, SO_ERROR, (int*)&sock_optval,
(socklen_t*)&sock_optval_size) != 0) {
// Connection failed
tcpclient_close(cli);
return -3;
}
if (sock_optval != 0) {
// Connection failed
tcpclient_close(cli);
return -4;
}
// Returns to blocking mode
set_block_mode(cli);
}
#endif
return 0;
}
int tcpclient_open(tcpclient_t* cli, const char* ip_str, int port_num)
{
enum { Connect_timeout_second = 2 };
fd_set rmask, wmask;
struct timeval tv = { Connect_timeout_second, 0 };
int flag;
int sock_optval = -1;
int sock_optval_size = sizeof(sock_optval);
int ret;
cli->sock_desc = Invalid_desc;
cli->pushed_back = -1; // no pushed back char.
tcpclient_buffer_init(cli);
cli->sock_addr_size = sizeof (struct sockaddr_in);
if ((cli->sock_desc = (int)socket(AF_INET, SOCK_STREAM, 0)) < 0) {
return -1;
}
memset((char*)&(cli->server_addr), 0, sizeof(cli->sock_addr_size));
cli->server_addr.sin_family = AF_INET;
cli->server_addr.sin_port = htons(port_num);
if (!strcmp(ip_str, "localhost")) {
ip_str = "127.0.0.1";
}
/* bind is not required, and port number is dynamic */
if ((cli->server_addr.sin_addr.s_addr = inet_addr(ip_str)) == INADDR_NONE) {
return -1;
}
flag = fcntl(cli->sock_desc, F_GETFL, 0);
fcntl(cli->sock_desc, F_SETFL, flag | O_NONBLOCK);
//printf("cli->sock_desc: %d\n",cli->sock_desc);
if (connect(cli->sock_desc, (const struct sockaddr *)&(cli->server_addr),
cli->sock_addr_size) < 0) {
if (errno != EINPROGRESS) {
tcpclient_close(cli);
return -1;
}
// EINPROGRESS:
FD_ZERO(&rmask);
FD_SET(cli->sock_desc, &rmask);
wmask = rmask;
ret = select(cli->sock_desc + 1, &rmask, &wmask, NULL, &tv);
if (ret <= 0) {
tcpclient_close(cli);
return -2;
}
if (getsockopt(cli->sock_desc, SOL_SOCKET, SO_ERROR, (int*)&sock_optval,
(socklen_t*)&sock_optval_size) != 0) {
tcpclient_close(cli);
return -3;
}
if (sock_optval != 0) {
tcpclient_close(cli);
return -4;
}
set_block_mode(cli);
}
//printf("connection ok!\n");
return 0;
}
