pilz_modbus_client_node.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 <algorithm>
19 #include <numeric>
20 #include <stdlib.h>
21 
22 #include <ros/ros.h>
23 
24 #include <pilz_utils/get_param.h>
31 
32 static constexpr int32_t MODBUS_CONNECTION_RETRIES_DEFAULT{ -1 };
33 static constexpr double MODBUS_CONNECTION_RETRY_TIMEOUT_S_DEFAULT{ 1.0 };
34 static constexpr int MODBUS_RESPONSE_TIMEOUT_MS{ 20 };
35 
36 using namespace prbt_hardware_support;
37 
41 int main(int argc, char** argv)
42 {
43  ros::init(argc, argv, "modbus_client_node");
44 
45  ros::NodeHandle pnh{ "~" };
46  ros::NodeHandle nh;
47 
48  // LCOV_EXCL_START Simple parameter reading not analyzed
49 
50  std::string ip;
51  int port;
52  std::vector<unsigned short> registers_to_read;
53 
54  try
55  {
56  ip = pilz_utils::getParam<std::string>(pnh, PARAM_MODBUS_SERVER_IP_STR);
57  port = pilz_utils::getParam<int>(pnh, PARAM_MODBUS_SERVER_PORT_STR);
58 
59  bool has_register_range_parameters =
61  if (has_register_range_parameters)
62  {
63  int num_registers_to_read = pilz_utils::getParam<int>(pnh, PARAM_NUM_REGISTERS_TO_READ_STR);
64  int index_of_first_register = pilz_utils::getParam<int>(pnh, PARAM_INDEX_OF_FIRST_REGISTER_TO_READ_STR);
65  registers_to_read = std::vector<unsigned short>(static_cast<unsigned long>(num_registers_to_read));
66  std::iota(registers_to_read.begin(), registers_to_read.end(), index_of_first_register);
67  }
68  else
69  {
70  ROS_INFO_STREAM("Parameters for register range are not set. Will try to determine range from api spec...");
71  ModbusApiSpec api_spec(nh);
72  api_spec.getAllDefinedRegisters(registers_to_read);
73  ROS_DEBUG("registers_to_read.size() %zu", registers_to_read.size());
74  }
75  }
76  catch (const std::runtime_error& ex)
77  {
78  ROS_ERROR_STREAM(ex.what());
79  return EXIT_FAILURE;
80  }
81 
82  int32_t modbus_connection_retries{ MODBUS_CONNECTION_RETRIES_DEFAULT };
83  pnh.param<int32_t>(PARAM_MODBUS_CONNECTION_RETRIES, modbus_connection_retries, MODBUS_CONNECTION_RETRIES_DEFAULT);
84 
85  double modbus_connection_retry_timeout_s{ MODBUS_CONNECTION_RETRY_TIMEOUT_S_DEFAULT };
86  pnh.param<double>(PARAM_MODBUS_CONNECTION_RETRY_TIMEOUT, modbus_connection_retry_timeout_s,
88 
89  int response_timeout_ms;
90  pnh.param<int>(PARAM_MODBUS_RESPONSE_TIMEOUT_STR, response_timeout_ms, MODBUS_RESPONSE_TIMEOUT_MS);
91 
92  std::string modbus_read_topic_name;
93  nh.param<std::string>(PARAM_MODBUS_READ_TOPIC_NAME_STR, modbus_read_topic_name, TOPIC_MODBUS_READ);
94 
95  std::string modbus_write_service_name;
96  nh.param<std::string>(PARAM_MODBUS_WRITE_SERVICE_NAME_STR, modbus_write_service_name, SERVICE_MODBUS_WRITE);
97 
98  // LCOV_EXCL_STOP
99 
101  pnh, registers_to_read, std::unique_ptr<LibModbusClient>(new LibModbusClient()),
102  static_cast<unsigned int>(response_timeout_ms), modbus_read_topic_name, modbus_write_service_name);
103 
104  ROS_DEBUG_STREAM("Modbus client IP: " << ip << " | Port: " << port);
105  std::ostringstream oss;
106  if (!registers_to_read.empty())
107  {
108  std::copy(registers_to_read.begin(), registers_to_read.end() - 1, std::ostream_iterator<unsigned short>(oss, ","));
109  oss << registers_to_read.back();
110  }
111  ROS_DEBUG_STREAM("Registers to read: " << oss.str());
112  ROS_DEBUG_STREAM("Modbus response timeout: " << response_timeout_ms);
113  ROS_DEBUG_STREAM("Modbus read topic: \"" << modbus_read_topic_name << "\"");
114  ROS_DEBUG_STREAM("Modbus write service: \"" << modbus_write_service_name << "\"");
115 
116  bool res = modbus_client.init(ip.c_str(), static_cast<unsigned int>(port), modbus_connection_retries,
117  ros::Duration(modbus_connection_retry_timeout_s));
118 
119  ROS_DEBUG_STREAM("Connection with modbus server " << ip << ":" << port << " established");
120 
121  // LCOV_EXCL_START inside this main ignored, tested multiple times in the unittest
122  if (!res)
123  {
124  ROS_ERROR_STREAM("Could not establish modbus connection with: "
125  << ip << ":" << port
126  << ". Make sure that your cables are connected properly and that "
127  "you have set the correct ip address and port.");
128 
129  return EXIT_FAILURE;
130  }
131 
132  try
133  {
134  modbus_client.run();
135  }
136  catch (PilzModbusClientException& e)
137  {
138  ROS_ERROR_STREAM(e.what());
139  return EXIT_FAILURE;
140  }
141  // LCOV_EXCL_STOP
142 
143  // If the client stop we don't want to kill this node since
144  // with required=true this would kill all nodes and no STOP1 would be performed in case of a disconnect.
145  ros::spin();
146 
147  return EXIT_SUCCESS;
148 }
static const std::string PARAM_MODBUS_SERVER_PORT_STR
Definition: param_names.h:26
void getAllDefinedRegisters(std::vector< unsigned short > &registers)
Expection thrown by prbt_hardware_support::PilzModbusClient.
static const std::string PARAM_MODBUS_RESPONSE_TIMEOUT_STR
Definition: param_names.h:27
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
static const std::string SERVICE_MODBUS_WRITE
static const std::string PARAM_MODBUS_WRITE_SERVICE_NAME_STR
Definition: param_names.h:29
static constexpr double MODBUS_CONNECTION_RETRY_TIMEOUT_S_DEFAULT
static const std::string TOPIC_MODBUS_READ
static constexpr int32_t MODBUS_CONNECTION_RETRIES_DEFAULT
bool param(const std::string &param_name, T &param_val, const T &default_val) const
static const std::string PARAM_MODBUS_READ_TOPIC_NAME_STR
Definition: param_names.h:28
Connects to a modbus server and publishes the received data into ROS.
ROSCPP_DECL void spin()
static const std::string PARAM_INDEX_OF_FIRST_REGISTER_TO_READ_STR
Definition: param_names.h:30
static constexpr int MODBUS_RESPONSE_TIMEOUT_MS
#define ROS_DEBUG_STREAM(args)
int main(int argc, char **argv)
Read requested parameters, start and initialize the prbt_hardware_support::PilzModbusClient.
#define ROS_INFO_STREAM(args)
static const std::string PARAM_MODBUS_CONNECTION_RETRIES
Definition: param_names.h:32
static const std::string PARAM_MODBUS_CONNECTION_RETRY_TIMEOUT
Definition: param_names.h:33
#define ROS_ERROR_STREAM(args)
Wrapper around libmodbus, see https://libmodbus.org/.
Specifies the meaning of the holding registers.
static const std::string PARAM_MODBUS_SERVER_IP_STR
Definition: param_names.h:25
#define ROS_DEBUG(...)
static const std::string PARAM_NUM_REGISTERS_TO_READ_STR
Definition: param_names.h:31


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