pilz_modbus_server_mock.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2018 Pilz GmbH & Co. KG
3  *
4  * This program is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU Lesser General Public License as published by
6  * the Free Software Foundation, either version 3 of the License, or
7  * (at your option) any later version.
8 
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details.
13 
14  * You should have received a copy of the GNU Lesser General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17 
18 #include <stdexcept>
19 
22 
23 #include <ros/console.h>
24 
25 namespace prbt_hardware_support
26 {
27 
28 PilzModbusServerMock::PilzModbusServerMock(const unsigned int& holding_register_size)
29  : holding_register_size_(holding_register_size + 1) // add one register for server shutdown signal
30  , terminate_register_idx_(holding_register_size)
31 {
32  // Create the needed mapping
33  // Please note: Only the holding register is used.
34  static constexpr unsigned BITS_NB {0x00};
35  static constexpr unsigned INPUT_BITS_NB {0x00};
36  static constexpr unsigned int INPUT_REGISTERS_NB {0x0};
37  mb_mapping_ = modbus_mapping_new(BITS_NB, INPUT_BITS_NB, static_cast<int>(holding_register_size_), INPUT_REGISTERS_NB);
38  if (mb_mapping_ == nullptr)
39  {
40  ROS_ERROR_NAMED("ServerMock", "mb_mapping_ is NULL.");
41  throw std::runtime_error("mb_mapping_ is NULL.");
42  }
43 }
44 
46 {
48  {
49  ROS_INFO_NAMED("ServerMock", "ModbusServer Mock is closing connection");
50  modbus_close(modbus_connection_);
51  modbus_free(modbus_connection_);
52  }
53 
54  modbus_mapping_free(mb_mapping_);
55 }
56 
57 bool PilzModbusServerMock::init(const char *ip, unsigned int port)
58 {
59  modbus_connection_ = modbus_new_tcp(ip, static_cast<int>(port));
60  if(modbus_connection_ == nullptr)
61  {
62  return false;
63  }
64  modbus_set_debug(modbus_connection_, true);
65 
66  ROS_DEBUG_STREAM_NAMED("ServerMock", "Starting Listening on Port " << ip << ":" << port);
67  return true;
68 }
69 
70 void PilzModbusServerMock::setHoldingRegister(std::initializer_list< std::pair<unsigned int, uint16_t> > reg_list)
71 {
72  for(auto entry : reg_list)
73  {
74  if (entry.first >= holding_register_size_ )
75  {
76  ROS_ERROR_STREAM_NAMED("ServerMock", "Holding Register is defined from: 0 ... " << (holding_register_size_-1) << "."
77  << " Setting Register " << entry.first << " ... " << " is not possible");
78  return;
79  }
80  }
81 
83  for(auto entry : reg_list)
84  {
85  mb_mapping_->tab_registers[entry.first] = entry.second;
86  }
88 
89  ROS_DEBUG_STREAM_NAMED("ServerMock", "Modbus data for Modbus-Server set.");
90 }
91 
92 void PilzModbusServerMock::setHoldingRegister(const RegCont& data, unsigned int start_index)
93 {
94  if ( data.empty() )
95  {
96  ROS_ERROR_NAMED("ServerMock", "No data given.");
97  return;
98  }
99 
100  if ( (start_index+data.size()) > holding_register_size_ )
101  {
102  ROS_ERROR_STREAM_NAMED("ServerMock", "Holding Register is defined from: 0 ... " << (holding_register_size_-1) << "."
103  << " Setting Register " << start_index << " ... " << start_index + data.size() - 1
104  << " is not possible");
105  return;
106  }
107 
108  ROS_DEBUG_STREAM_NAMED("ServerMock", "Set Modbus data for Modbus-Server...");
110  for (size_t index = 0; index < data.size(); ++index)
111  {
112  mb_mapping_->tab_registers[start_index+index] = data.at(index);
113  }
115  ROS_DEBUG_STREAM_NAMED("ServerMock", "Modbus data for Modbus-Server set.");
116 }
117 
118 RegCont PilzModbusServerMock::readHoldingRegister(const RegCont::size_type start_index,
119  const RegCont::size_type num_reg_to_read)
120 {
121  RegCont ret_val(num_reg_to_read, 0);
123  for (RegCont::size_type i = 0; i < num_reg_to_read; ++i)
124  {
125  ret_val.at(i) = mb_mapping_->tab_registers[start_index + i];
126  }
128  return ret_val;
129 }
130 
131 void PilzModbusServerMock::start(const char* ip, const unsigned int port)
132 {
133  init(ip, port);
134  run();
135 }
136 
137 void PilzModbusServerMock::startAsync(const char* ip, const unsigned int port)
138 {
139  std::unique_lock<std::mutex> lk(running_mutex_);
140 
141  thread_ = std::thread{ [this, ip, port]{
142  this->start(ip, port);
143  }};
144 
145  running_cv_.wait(lk);
146 }
147 
149 {
150 
151 #if LIBMODBUS_VERSION_CHECK(3,1,2) // API changed from timeval to uint,uint) in version >= 3.1.2
153 #else
154  struct timeval response_timeout;
155  response_timeout.tv_sec = RESPONSE_TIMEOUT_IN_SEC;
156  response_timeout.tv_usec = RESPONSE_TIMEOUT_IN_USEC;
157  modbus_set_response_timeout(modbus_connection_, &response_timeout);
158 #endif
159 
160  uint8_t query[MODBUS_TCP_MAX_ADU_LENGTH];
161 
162  // Connect to client loop
163  while (!terminate_ && !shutdownSignalReceived())
164  {
165  ROS_DEBUG_NAMED("ServerMock", "About to start to listen for a modbus connection");
166  socket_ = modbus_tcp_listen(modbus_connection_, 1);
167  // Set socket non blocking
168  //fcntl(socket_, F_SETFL, O_NONBLOCK);
169 
170  {
171  std::lock_guard<std::mutex> lk(running_mutex_);
172  running_cv_.notify_one();
173  }
174 
175  ROS_INFO_NAMED("ServerMock", "Waiting for connection...");
176  int result {-1};
177  while(result < 0)
178  {
179  result = modbus_tcp_accept(modbus_connection_, &socket_);
180  ROS_DEBUG_NAMED("ServerMock", "Accepted new connection, result: %i", result);
181 
182  if(terminate_) break;
183  }
184  ROS_INFO_NAMED("ServerMock", "Connection with client accepted.");
185 
186  // Loop for reading
187  int rc {-1};
188  while (!terminate_ && !shutdownSignalReceived())
189  {
190  rc = modbus_receive(modbus_connection_, query);
191  if(rc > 0)
192  {
194  modbus_reply(modbus_connection_, query, rc, mb_mapping_);
196  }
197  else
198  {
199  ROS_INFO_NAMED("ServerMock", "Connection with client closed");
200  break;
201  }
202 
203  // Allow other threads to run.
204  usleep(50);
205  } // End reading loop
206 
207  close(socket_);
208  usleep(50); // Avoid issues with modbus_tcp_listen since socket not closed
209  ROS_DEBUG_NAMED("ServerMock", "Socket closed");
210  socket_ = -1;
211  } // End connect to client loop
212  ROS_INFO_NAMED("ServerMock", "Modbus-server run loop finished.");
213 
214 }
215 
216 } // namespace prbt_hardware_support
#define ROS_INFO_NAMED(name,...)
bool init(const char *ip, unsigned int port)
Allocates needed resources for running the server.
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_ERROR_STREAM_NAMED(name, args)
std::vector< uint16_t > RegCont
Convenience data type defining the data type for a collection of registers.
RegCont readHoldingRegister(const RegCont::size_type start_index, const RegCont::size_type num_reg_to_read)
Reads the specified number of registers, beginning at the specified start point from the holding regi...
void startAsync(const char *ip, const unsigned int port)
Start the modbus server asynchronously and make it accessible for clients.
void setHoldingRegister(std::initializer_list< std::pair< unsigned int, uint16_t > > reg_list)
void run()
Run the server and publish the register values as messages. The value of the modbus register is read ...
#define ROS_DEBUG_NAMED(name,...)
void start(const char *ip, const unsigned int port)
Start the modbus server and make it accessible for clients.
#define ROS_ERROR_NAMED(name,...)
PilzModbusServerMock(const unsigned int &holding_register_size)


prbt_hardware_support
Author(s):
autogenerated on Thu Mar 19 2020 03:13:27