libmodbus_client.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 <ros/ros.h>
19 
20 #include <cstddef>
21 #include <vector>
22 #include <errno.h>
23 #include <limits>
24 #include <thread>
28 
29 namespace prbt_hardware_support
30 {
32 {
33  close();
34 }
35 
36 bool LibModbusClient::init(const char* ip, unsigned int port)
37 {
38  // The following check results from Ubuntu 18.04 using libmodbus 3.0.6 where a timeout cannot be set on
39  // modbus_connect.
40  // As a result trying to connect with to a wrong address could modbus_connect could get stuck for up to over 100
41  // seconds. The precheck lowers this risk by performing a quick connect to the given address.
42  //
43  // If you read this comment at a time where Ubuntu 18.04 is no longer relevant please remove this check and define a
44  // timeout for modbus_connect(). Thank you!
45  if (!checkIPConnection(ip, port))
46  {
47  ROS_ERROR_STREAM("Precheck for connection to " << ip << ":" << port << " failed. " << modbus_strerror(errno)
48  << ".");
49  return false;
50  }
51 
52  modbus_connection_ = modbus_new_tcp(ip, static_cast<int>(port));
53 
54  if (modbus_connect(modbus_connection_) == -1)
55  {
56  // LCOV_EXCL_START the following lines are hard to cover since the above checkIPConnection should prevent
57  // exactly this situation
58  ROS_ERROR_STREAM_NAMED("LibModbusClient",
59  "Could not establish modbus connection. " << modbus_strerror(errno) << ".");
60  modbus_free(modbus_connection_);
61  modbus_connection_ = nullptr;
62  return false;
63  // LCOV_EXCL_STOP
64  }
65 
66  return true;
67 }
68 
69 void LibModbusClient::setResponseTimeoutInMs(unsigned long timeout_ms)
70 {
71  struct timeval response_timeout;
72  response_timeout.tv_sec = timeout_ms / 1000;
73  response_timeout.tv_usec = (timeout_ms % 1000) * 1000;
74  modbus_set_response_timeout(modbus_connection_, &response_timeout);
75 }
76 
78 {
79  struct timeval response_timeout;
80  modbus_get_response_timeout(modbus_connection_, &response_timeout);
81  return static_cast<unsigned long>(response_timeout.tv_sec * 1000L + (response_timeout.tv_usec / 1000L));
82 }
83 
85 {
86  ROS_DEBUG("readHoldingRegister()");
87  if (modbus_connection_ == nullptr)
88  {
89  throw ModbusExceptionDisconnect("Modbus disconnected!");
90  }
91 
92  RegCont tab_reg(static_cast<RegCont::size_type>(nb));
93  int rc{ -1 };
94 
95  rc = modbus_read_registers(modbus_connection_, addr, nb, tab_reg.data());
96  if (rc == -1)
97  {
98  std::ostringstream err_stream;
99  err_stream << "Failed to read " << nb;
100  err_stream << " registers starting from " << addr;
101  err_stream << " with err: " << modbus_strerror(errno);
102  ROS_ERROR_STREAM_NAMED("LibModbusClient", err_stream.str());
103  throw ModbusExceptionDisconnect(err_stream.str());
104  }
105 
106  return tab_reg;
107 }
108 
109 RegCont LibModbusClient::writeReadHoldingRegister(const int write_addr, const RegCont& write_reg, const int read_addr,
110  const int read_nb)
111 {
112  if (modbus_connection_ == nullptr)
113  {
114  throw ModbusExceptionDisconnect("Modbus disconnected!");
115  }
116 
117  if (read_nb < 0)
118  {
119  throw std::invalid_argument("Argument \"read_nb\" must not be negative");
120  }
121  RegCont read_reg(static_cast<RegCont::size_type>(read_nb));
122 
123  if (write_reg.size() > std::numeric_limits<int>::max())
124  {
125  throw std::invalid_argument("Argument \"write_reg\" must not exceed max value of type \"int\"");
126  }
127 
128  int rc{ -1 };
129  rc = modbus_write_and_read_registers(modbus_connection_, write_addr, static_cast<int>(write_reg.size()),
130  write_reg.data(), read_addr, read_nb, read_reg.data());
131  ROS_DEBUG_NAMED("LibModbusClient", "modbus_write_and_read_registers: writing from %i %i registers\
132  and reading from %i %i registers",
133  write_addr, static_cast<int>(write_reg.size()), read_addr, read_nb);
134  if (rc == -1)
135  {
136  std::string err = "Failed to write and read modbus registers: ";
137  err.append(modbus_strerror(errno));
138  ROS_ERROR_STREAM_NAMED("LibModbusClient", err);
139  throw ModbusExceptionDisconnect(err);
140  }
141 
142  return read_reg;
143 }
144 
146 {
147  if (modbus_connection_)
148  {
149  modbus_close(modbus_connection_);
150  modbus_free(modbus_connection_);
151  modbus_connection_ = nullptr;
152  }
153 }
154 
155 } // namespace prbt_hardware_support
virtual ~LibModbusClient() override
See base class.
#define ROS_ERROR_STREAM_NAMED(name, args)
RegCont readHoldingRegister(int addr, int nb) override
See base class.
std::vector< uint16_t > RegCont
Convenience data type defining the data type for a collection of registers.
void close()
Close connection with server.
bool init(const char *ip, unsigned int port) override
See base class.
RegCont writeReadHoldingRegister(const int write_addr, const RegCont &write_reg, const int read_addr, const int read_nb) override
See base class.
#define ROS_DEBUG_NAMED(name,...)
void setResponseTimeoutInMs(unsigned long timeout_ms) override
See base class.
unsigned long getResponseTimeoutInMs() override
See base class.
bool checkIPConnection(const char *ip, const unsigned int &port)
Test the ip connection by connecting to the modbus server.
#define ROS_ERROR_STREAM(args)
Expection thrown by prbt_hardware_support::LibModbusClient::readHoldingRegister if a disconnect from ...
#define ROS_DEBUG(...)


prbt_hardware_support
Author(s):
autogenerated on Thu Sep 10 2020 03:14:36