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 PilzModbusServerMock::PilzModbusServerMock(const unsigned int& holding_register_size)
28  : holding_register_size_(holding_register_size + 1) // add one register for server shutdown signal
29  , terminate_register_idx_(holding_register_size)
30 {
31  // Create the needed mapping
32  // Please note: Only the holding register is used.
33  static constexpr unsigned BITS_NB{ 0x00 };
34  static constexpr unsigned INPUT_BITS_NB{ 0x00 };
35  static constexpr unsigned int INPUT_REGISTERS_NB{ 0x0 };
36  mb_mapping_ =
37  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 ... "
77  << (holding_register_size_ - 1) << "."
78  << " Setting Register " << entry.first << " ... "
79  << " is not possible");
80  return;
81  }
82  }
83 
85  for (auto entry : reg_list)
86  {
87  mb_mapping_->tab_registers[entry.first] = entry.second;
88  }
90 
91  ROS_DEBUG_STREAM_NAMED("ServerMock", "Modbus data for Modbus-Server set.");
92 }
93 
94 void PilzModbusServerMock::setHoldingRegister(const RegCont& data, unsigned int start_index)
95 {
96  if (data.empty())
97  {
98  ROS_ERROR_NAMED("ServerMock", "No data given.");
99  return;
100  }
101 
102  if ((start_index + data.size()) > holding_register_size_)
103  {
104  ROS_ERROR_STREAM_NAMED("ServerMock", "Holding Register is defined from: 0 ... "
105  << (holding_register_size_ - 1) << "."
106  << " Setting Register " << start_index << " ... "
107  << start_index + data.size() - 1 << " is not possible");
108  return;
109  }
110 
111  ROS_DEBUG_STREAM_NAMED("ServerMock", "Set Modbus data for Modbus-Server...");
113  for (size_t index = 0; index < data.size(); ++index)
114  {
115  mb_mapping_->tab_registers[start_index + index] = data.at(index);
116  }
118  ROS_DEBUG_STREAM_NAMED("ServerMock", "Modbus data for Modbus-Server set.");
119 }
120 
121 RegCont PilzModbusServerMock::readHoldingRegister(const RegCont::size_type start_index,
122  const RegCont::size_type num_reg_to_read)
123 {
124  RegCont ret_val(num_reg_to_read, 0);
126  for (RegCont::size_type i = 0; i < num_reg_to_read; ++i)
127  {
128  ret_val.at(i) = mb_mapping_->tab_registers[start_index + i];
129  }
131  return ret_val;
132 }
133 
134 void PilzModbusServerMock::start(const char* ip, const unsigned int port)
135 {
136  init(ip, port);
137  run();
138 }
139 
140 void PilzModbusServerMock::startAsync(const char* ip, const unsigned int port)
141 {
142  std::unique_lock<std::mutex> lk(running_mutex_);
143 
144  thread_ = std::thread{ [this, ip, port] { this->start(ip, port); } };
145 
146  running_cv_.wait(lk);
147 }
148 
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_)
183  break;
184  }
185  ROS_INFO_NAMED("ServerMock", "Connection with client accepted.");
186 
187  // Loop for reading
188  int rc{ -1 };
189  while (!terminate_ && !shutdownSignalReceived())
190  {
191  rc = modbus_receive(modbus_connection_, query);
192  if (rc > 0)
193  {
195  modbus_reply(modbus_connection_, query, rc, mb_mapping_);
197  }
198  else
199  {
200  ROS_INFO_NAMED("ServerMock", "Connection with client closed");
201  break;
202  }
203 
204  // Allow other threads to run.
205  usleep(50);
206  } // End reading loop
207 
208  close(socket_);
209  usleep(50); // Avoid issues with modbus_tcp_listen since socket not closed
210  ROS_DEBUG_NAMED("ServerMock", "Socket closed");
211  socket_ = -1;
212  } // End connect to client loop
213  ROS_INFO_NAMED("ServerMock", "Modbus-server run loop finished.");
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 Feb 4 2021 03:09:23