pilz_modbus_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 
19 
20 #include <prbt_hardware_support/ModbusMsgInStamped.h>
24 
25 namespace prbt_hardware_support
26 {
27 PilzModbusClient::PilzModbusClient(ros::NodeHandle& nh, const std::vector<unsigned short>& registers_to_read,
28  ModbusClientUniquePtr modbus_client, unsigned int response_timeout_ms,
29  const std::string& modbus_read_topic_name,
30  const std::string& modbus_write_service_name, double read_frequency_hz)
31  : registers_to_read_(registers_to_read)
32  , RESPONSE_TIMEOUT_MS(response_timeout_ms)
33  , READ_FREQUENCY_HZ(read_frequency_hz)
34  , modbus_client_(std::move(modbus_client))
35  , modbus_read_pub_(nh.advertise<ModbusMsgInStamped>(modbus_read_topic_name, DEFAULT_QUEUE_SIZE_MODBUS))
36  , modbus_write_service_(
37  nh.advertiseService(modbus_write_service_name, &PilzModbusClient::modbus_write_service_cb, this))
38 {
39 }
40 
41 bool PilzModbusClient::init(const char* ip, unsigned int port, int retries, const ros::Duration& timeout)
42 {
43  size_t retry_n = 0;
44  while (ros::ok() && (retries == -1 || static_cast<int>(retry_n) < retries))
45  {
46  ++retry_n;
47 
48  if (init(ip, port))
49  {
50  // The following warning needs only to be shown to complete the warnings shown if this didn't work on the first
51  // try
52  ROS_WARN_STREAM_COND(retry_n > 1, "Connection to " << ip << ":" << port << " successful.");
53  return true;
54  }
55 
56  ROS_WARN_STREAM_COND(retries == -1, "Connection to "
57  << ip << ":" << port
58  << " failed. Make sure that your cables are connected properly and that "
59  "you have set the correct ip address and port.");
60  ROS_WARN_STREAM_COND(retries != -1, "Connection to "
61  << ip << ":" << port << " failed. Try(" << retry_n << "/" << retries
62  << "). Make sure that your cables are connected properly and that "
63  "you have set the correct ip address and port.");
64  timeout.sleep();
65  }
66 
67  return false;
68 }
69 
70 bool PilzModbusClient::init(const char* ip, unsigned int port)
71 {
72  State expected_state{ State::not_initialized };
73  if (!state_.compare_exchange_strong(expected_state, State::initializing))
74  {
75  ROS_ERROR_STREAM("Modbus-client not in correct state: " << state_ << "expected:" << State::initializing);
76  state_ = State::not_initialized;
77  return false;
78  }
79 
80  if (!modbus_client_->init(ip, port))
81  {
82  ROS_DEBUG("Init failed");
83  state_ = State::not_initialized;
84  return false;
85  }
86 
87  modbus_client_->setResponseTimeoutInMs(RESPONSE_TIMEOUT_MS);
88 
89  state_ = State::initialized;
90  ROS_DEBUG_STREAM("Connection to " << ip << ":" << port << " established");
91  return true;
92 }
93 
95 {
96  ModbusMsgInStamped msg;
97  msg.disconnect.data = true;
98  msg.header.stamp = ros::Time::now();
100  ros::spinOnce();
101 }
102 
104 {
105  State expected_state{ State::initialized };
106  if (!state_.compare_exchange_strong(expected_state, State::running))
107  {
108  ROS_ERROR_STREAM("Modbus-client not in correct state: " << state_ << "expected:" << State::running);
109  throw PilzModbusClientException("Modbus-client not in correct state.");
110  }
111 
112  RegCont holding_register;
113  RegCont last_holding_register;
114  ros::Time last_update{ ros::Time::now() };
115  state_ = State::running;
117  while (ros::ok() && !stop_run_.load())
118  {
119  // Work with local copy of buffer to ensure that the service callback
120  // function does not become blocked
121  boost::optional<ModbusRegisterBlock> write_reg_bock{ boost::none };
122  {
123  std::lock_guard<std::mutex> lock(write_reg_blocks_mutex_);
124  if (!write_reg_blocks_.empty())
125  {
126  write_reg_bock = write_reg_blocks_.front();
127  // Mark data as send/processed, by "deleting" them from memory
128  write_reg_blocks_.pop();
129  }
130  }
131 
132  std::vector<std::vector<unsigned short>> blocks = splitIntoBlocks(registers_to_read_);
133 
134  unsigned short index_of_first_register = *std::min_element(registers_to_read_.begin(), registers_to_read_.end());
135  int num_registers =
136  *std::max_element(registers_to_read_.begin(), registers_to_read_.end()) - index_of_first_register + 1;
137  holding_register = RegCont(static_cast<unsigned long>(num_registers), 0);
138 
139  ROS_DEBUG("blocks.size() %zu", blocks.size());
140  try
141  {
142  for (auto& block : blocks)
143  {
144  ROS_DEBUG("block.size() %zu", block.size());
145  unsigned short index_of_first_register_block = *(block.begin());
146  unsigned long num_registers_block = block.size();
147  RegCont block_holding_register;
148  if (write_reg_bock)
149  {
150  block_holding_register = modbus_client_->writeReadHoldingRegister(
151  static_cast<int>(write_reg_bock->start_idx), write_reg_bock->values,
152  static_cast<int>(index_of_first_register_block), static_cast<int>(num_registers_block));
153  // write only once:
154  write_reg_bock = boost::none;
155  }
156  else
157  {
158  block_holding_register = modbus_client_->readHoldingRegister(static_cast<int>(index_of_first_register_block),
159  static_cast<int>(num_registers_block));
160  }
161  for (uint i = 0; i < num_registers_block; i++)
162  holding_register[i + index_of_first_register_block - index_of_first_register] = block_holding_register[i];
163  }
164  }
165  catch (ModbusExceptionDisconnect& e)
166  {
167  ROS_ERROR_STREAM("Modbus disconnect: " << e.what());
169  break;
170  }
171 
172  ModbusMsgInStampedPtr msg{ ModbusMsgInBuilder::createDefaultModbusMsgIn(index_of_first_register,
173  holding_register) };
174 
175  // Publish the received data into ROS
176  if (holding_register != last_holding_register)
177  {
178  ROS_DEBUG_STREAM("Sending new ROS-message.");
179  msg->header.stamp = ros::Time::now();
180  last_update = msg->header.stamp;
181  last_holding_register = holding_register;
182  }
183  else
184  {
185  msg->header.stamp = last_update;
186  }
188 
189  ros::spinOnce();
190  rate.sleep();
191  }
192 
193  stop_run_ = false;
194  state_ = State::not_initialized;
195 }
196 
197 std::vector<std::vector<unsigned short>> PilzModbusClient::splitIntoBlocks(std::vector<unsigned short>& in)
198 {
199  std::vector<std::vector<unsigned short>> out;
200  std::sort(in.begin(), in.end()); // sort just in case to be more user-friendly
201  unsigned short prev{ 0 };
202  std::vector<unsigned short> current_block;
203  for (auto& reg : in)
204  {
205  if (reg == prev)
206  {
207  throw PilzModbusClientException("List elemts must be unique.");
208  }
209  else if (reg == prev + 1)
210  {
211  current_block.push_back(reg);
212  }
213  else
214  { // *it >= prev + 1
215  std::vector<unsigned short> to_out(current_block);
216  if (!to_out.empty())
217  {
218  out.push_back(to_out);
219  }
220  current_block.clear();
221  current_block.push_back(reg);
222  }
223  prev = reg;
224  }
225  std::vector<unsigned short> to_out(current_block);
226  if (!to_out.empty())
227  {
228  out.push_back(to_out);
229  }
230  return out;
231 }
232 
233 } // namespace prbt_hardware_support
msg
std::vector< unsigned short > registers_to_read_
Registers which have to be read.
void run()
Publishes the register values as messages.
std::vector< uint16_t > RegCont
Convenience data type defining the data type for a collection of registers.
Expection thrown by prbt_hardware_support::PilzModbusClient.
bool init(const char *ip, unsigned int port)
Tries to connect to a modbus server.
ServiceServer advertiseService(ros::NodeHandle n, std::string name, boost::function< bool(const typename ActionSpec::_action_goal_type::_goal_type &, typename ActionSpec::_action_result_type::_result_type &result)> service_cb)
static std::vector< std::vector< unsigned short > > splitIntoBlocks(std::vector< unsigned short > &in)
Splits a vector of integers into a vector of vectors with consecutive groups.
const unsigned int RESPONSE_TIMEOUT_MS
Defines how long we wait for a response from the Modbus-server.
void publish(const boost::shared_ptr< M > &message) const
PilzModbusClient(ros::NodeHandle &nh, const std::vector< unsigned short > &registers_to_read, ModbusClientUniquePtr modbus_client, unsigned int response_timeout_ms, const std::string &modbus_read_topic_name, const std::string &modbus_write_service_name, double read_frequency_hz=DEFAULT_MODBUS_READ_FREQUENCY_HZ)
Sets up publisher. To open the modbus connection call PilzModbusClient::init.
static ModbusMsgInStampedPtr createDefaultModbusMsgIn(const std_msgs::MultiArrayLayout::_data_offset_type &offset, const RegCont &holding_register)
Creates a standard ModbusMsgIn which contains default values for all essential elements of the messag...
Connects to a modbus server and publishes the received data into ROS.
ROSCPP_DECL bool ok()
#define ROS_WARN_STREAM_COND(cond, args)
#define ROS_DEBUG_STREAM(args)
bool sleep()
static Time now()
std::unique_ptr< ModbusClient > ModbusClientUniquePtr
std::queue< ModbusRegisterBlock > write_reg_blocks_
bool sleep() const
#define ROS_ERROR_STREAM(args)
ROSCPP_DECL void spinOnce()
Expection thrown by prbt_hardware_support::LibModbusClient::readHoldingRegister if a disconnect from ...
const double READ_FREQUENCY_HZ
Defines how often the Modbus registers are read in.
#define ROS_DEBUG(...)


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