diff --git a/config/params.yaml b/config/params.yaml
index b963ce631b013db7a4fc3bf92ad850d3d2228e40..e87b4ce50cd21f4f7e5366122636fd509d8d9bfc 100644
--- a/config/params.yaml
+++ b/config/params.yaml
@@ -1,5 +1,5 @@
rate: 10
-modbus_address: 127.0.0.1
+modbus_address: 10.70.13.77
modbus_port: 5020
map_frame: map
home_x: 0.0
diff --git a/include/looming_modbus_driver.h b/include/looming_modbus_driver.h
index 8350e1303e5c700e50272e42ea8475c4ce7cc940..9ec6e53a736584ebb31509dc4aaed55e0141cfe2 100644
--- a/include/looming_modbus_driver.h
+++ b/include/looming_modbus_driver.h
@@ -60,19 +60,22 @@ class LoomingModbusDriver : public iri_base_driver::IriBaseDriver
modbus_t *context;
modbus_mapping_t *mapping;
int server_socket;
- int client_socket;
- bool connected;
+ std::vector<int> client_sockets;
int status;
CMutex access;
/* thread attributes */
CThreadServer *thread_server;
std::string request_thread_id;
+ std::string new_clients_thread_id;
/* event attributes */
CEventServer *event_server;
- std::string finish_thread_event_id;
+ std::string finish_request_thread_event_id;
+ std::string finish_new_clients_thread_event_id;
+ CEvent new_client_event;
protected:
static void *request_thread(void *param);
+ static void *new_clients_thread(void *param);
public:
/**
@@ -164,7 +167,6 @@ class LoomingModbusDriver : public iri_base_driver::IriBaseDriver
// here define all looming_modbus_driver interface methods to retrieve and set
// the driver parameters
- bool client_connected(void);
void set_ready(void);
void set_navigating(void);
void set_success(void);
diff --git a/src/looming_modbus_driver.cpp b/src/looming_modbus_driver.cpp
index 2b1c9a956040eddb9a1bd3448fd3953c56072123..597ad5546412378bdc0d1cf5884bfa58a8a7a3cb 100644
--- a/src/looming_modbus_driver.cpp
+++ b/src/looming_modbus_driver.cpp
@@ -2,23 +2,28 @@
#include "looming_registers.h"
#include <sys/select.h>
-LoomingModbusDriver::LoomingModbusDriver(void)
+LoomingModbusDriver::LoomingModbusDriver(void) : new_client_event("modbus_new_client_event")
{
//setDriverId(driver string id);
this->server_socket=-1;
this->context=NULL;
this->mapping=NULL;
- this->connected=false;
this->status=0x03;
+ this->client_sockets.clear();
/* initialize events */
this->event_server=CEventServer::instance();
- this->finish_thread_event_id="modbus_finish_thread_event";
- this->event_server->create_event(this->finish_thread_event_id);
+ this->finish_request_thread_event_id="modbus_finish_request_thread_event";
+ this->event_server->create_event(this->finish_request_thread_event_id);
+ this->finish_new_clients_thread_event_id="modbus_finish_new_clients_thread_event";
+ this->event_server->create_event(this->finish_new_clients_thread_event_id);
/* initialize threads */
this->thread_server=CThreadServer::instance();
this->request_thread_id="modbus_request_thread";
this->thread_server->create_thread(this->request_thread_id);
this->thread_server->attach_thread(this->request_thread_id,this->request_thread,this);
+ this->new_clients_thread_id="new_clients_thread";
+ this->thread_server->create_thread(this->new_clients_thread_id);
+ this->thread_server->attach_thread(this->new_clients_thread_id,this->new_clients_thread,this);
}
bool LoomingModbusDriver::openDriver(void)
@@ -90,6 +95,7 @@ bool LoomingModbusDriver::startDriver(void)
}
// start thread
this->thread_server->start_thread(this->request_thread_id);
+ this->thread_server->start_thread(this->new_clients_thread_id);
return true;
}
@@ -99,9 +105,15 @@ bool LoomingModbusDriver::stopDriver(void)
if(this->thread_server->get_thread_state(this->request_thread_id)==starting ||
this->thread_server->get_thread_state(this->request_thread_id)==active)
{
- this->event_server->set_event(this->finish_thread_event_id);
+ this->event_server->set_event(this->finish_request_thread_event_id);
this->thread_server->end_thread(this->request_thread_id);
}
+ if(this->thread_server->get_thread_state(this->new_clients_thread_id)==starting ||
+ this->thread_server->get_thread_state(this->new_clients_thread_id)==active)
+ {
+ this->event_server->set_event(this->finish_new_clients_thread_event_id);
+ this->thread_server->end_thread(this->new_clients_thread_id);
+ }
// close socket
if(this->server_socket!=-1)
close(this->server_socket);
@@ -132,87 +144,111 @@ void LoomingModbusDriver::config_update(Config& new_cfg, uint32_t level)
this->unlock();
}
-void *LoomingModbusDriver::request_thread(void *param)
+void *LoomingModbusDriver::new_clients_thread(void *param)
{
LoomingModbusDriver *modbus=(LoomingModbusDriver *)param;
fd_set receive_set;
timeval time;
- uint8_t req[MODBUS_TCP_MAX_ADU_LENGTH];
- int num;
+ int new_client_socket,num;
bool end=false;
while(!end)
{
- if(modbus->connected==false)
+ FD_ZERO(&receive_set);
+ FD_SET(modbus->server_socket,&receive_set);
+ time.tv_sec=0;
+ time.tv_usec=100000;
+ if((num=select(modbus->server_socket+1,&receive_set,NULL,NULL,&time))!=-1)
{
- FD_ZERO(&receive_set);
- FD_SET(modbus->server_socket,&receive_set);
- time.tv_sec=0;
- time.tv_usec=100000;
- if((num=select(modbus->server_socket+1,&receive_set,NULL,NULL,&time))!=-1)
+ if(FD_ISSET(modbus->server_socket,&receive_set))
{
- if(FD_ISSET(modbus->server_socket,&receive_set))
+ if((new_client_socket=modbus_tcp_accept(modbus->context,&modbus->server_socket))==-1)
+ ROS_WARN_STREAM("Impossible to accept client connection: " << modbus_strerror(errno));
+ else
{
- if((modbus->client_socket=modbus_tcp_accept(modbus->context,&modbus->server_socket))==-1)
- ROS_WARN_STREAM("Impossible to accept client connection: " << modbus_strerror(errno));
- else
- {
- modbus->access.enter();
- std::cout << "connected" << std::endl;
- modbus->connected=true;
- modbus->access.exit();
- }
+ modbus->access.enter();
+ std::cout << "connected new client: " << new_client_socket << std::endl;
+ modbus->client_sockets.push_back(new_client_socket);
+ modbus->new_client_event.set();
+ modbus->access.exit();
}
}
}
- else
+ if(modbus->event_server->event_is_set(modbus->finish_new_clients_thread_event_id))
+ end=true;
+ }
+
+ pthread_exit(NULL);
+}
+
+void *LoomingModbusDriver::request_thread(void *param)
+{
+ LoomingModbusDriver *modbus=(LoomingModbusDriver *)param;
+ fd_set receive_set;
+ timeval time;
+ uint8_t req[MODBUS_TCP_MAX_ADU_LENGTH];
+ int num,max_id;
+ bool end=false;
+
+ FD_ZERO(&receive_set);
+ FD_SET(modbus->new_client_event.get_fd(),&receive_set);
+ max_id=modbus->new_client_event.get_fd()+1;
+ time.tv_sec=0;
+ time.tv_usec=100000;
+ while(!end)
+ {
+ if((num=select(max_id,&receive_set,NULL,NULL,&time))!=-1)
{
- // handle request
- FD_ZERO(&receive_set);
- FD_SET(modbus->client_socket,&receive_set);
- time.tv_sec=0;
- time.tv_usec=1000000;
- if((num=select(modbus->client_socket+1,&receive_set,NULL,NULL,&time))!=-1)
+ modbus->access.enter();
+ for(std::vector<int>::iterator it=modbus->client_sockets.begin();it!=modbus->client_sockets.end();)
{
- if(FD_ISSET(modbus->client_socket,&receive_set))
- {
- modbus->access.enter();
- modbus_set_socket(modbus->context,modbus->client_socket);
+ if(FD_ISSET(*it,&receive_set))
+ {
+ modbus_set_socket(modbus->context,*it);
if((num=modbus_receive(modbus->context,req))==-1)
{
- ROS_WARN_STREAM("Error while receiving request: " << modbus_strerror(errno));
- close(modbus->client_socket);
- modbus->connected=false;
+ ROS_WARN_STREAM("Error while receiving request on socket " << *it << ": " << modbus_strerror(errno));
+ close(*it);
+ it=modbus->client_sockets.erase(it);
}
else if(num>0)
{
modbus->mapping->tab_input_registers[STATUS_REG]=modbus->status;
modbus_reply(modbus->context,req,num,modbus->mapping);
+ it++;
}
else
{
- ROS_WARN_STREAM("Client disconnected");
- close(modbus->client_socket);
- modbus->connected=false;
+ ROS_WARN_STREAM("Client socket " << *it << " disconnected");
+ close(*it);
+ it=modbus->client_sockets.erase(it);
}
- modbus->access.exit();
- }
+ }
+ else
+ it++;
}
- else
- ROS_WARN_STREAM("timeout while waiting to receive a request" << modbus_strerror(errno));
+ if(modbus->new_client_event.is_set())
+ modbus->new_client_event.reset();
+ FD_ZERO(&receive_set);
+ FD_SET(modbus->new_client_event.get_fd(),&receive_set);
+ max_id=modbus->new_client_event.get_fd()+1;
+ for(unsigned int i=0;i<modbus->client_sockets.size();i++)
+ {
+ FD_SET(modbus->client_sockets[i],&receive_set);
+ if((modbus->client_sockets[i]+1)>max_id)
+ max_id=modbus->client_sockets[i]+1;
+ }
+ modbus->access.exit();
}
- if(modbus->event_server->event_is_set(modbus->finish_thread_event_id))
+ time.tv_sec=0;
+ time.tv_usec=100000;
+ if(modbus->event_server->event_is_set(modbus->finish_request_thread_event_id))
end=true;
}
pthread_exit(NULL);
}
-bool LoomingModbusDriver::client_connected(void)
-{
- return this->connected;
-}
-
void LoomingModbusDriver::set_ready(void)
{
if(this->mapping!=NULL)
@@ -321,12 +357,23 @@ LoomingModbusDriver::~LoomingModbusDriver(void)
if(this->thread_server->get_thread_state(this->request_thread_id)==starting ||
this->thread_server->get_thread_state(this->request_thread_id)==active)
{
- this->event_server->set_event(this->finish_thread_event_id);
+ this->event_server->set_event(this->finish_request_thread_event_id);
this->thread_server->end_thread(this->request_thread_id);
}
this->thread_server->detach_thread(this->request_thread_id);
this->thread_server->delete_thread(this->request_thread_id);
this->request_thread_id="";
- this->event_server->delete_event(this->finish_thread_event_id);
- this->finish_thread_event_id="";
+ if(this->thread_server->get_thread_state(this->new_clients_thread_id)==starting ||
+ this->thread_server->get_thread_state(this->new_clients_thread_id)==active)
+ {
+ this->event_server->set_event(this->finish_new_clients_thread_event_id);
+ this->thread_server->end_thread(this->request_thread_id);
+ }
+ this->thread_server->detach_thread(this->new_clients_thread_id);
+ this->thread_server->delete_thread(this->new_clients_thread_id);
+ this->new_clients_thread_id="";
+ this->event_server->delete_event(this->finish_request_thread_event_id);
+ this->finish_request_thread_event_id="";
+ this->event_server->delete_event(this->finish_new_clients_thread_event_id);
+ this->finish_new_clients_thread_event_id="";
}