modbus_adapter_brake_test.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2019 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 #ifndef MODBUS_ADAPTER_BRAKE_TEST_H
19 #define MODBUS_ADAPTER_BRAKE_TEST_H
20 
21 #include <memory>
22 #include <map>
23 #include <string>
24 #include <functional>
25 
26 #include <ros/ros.h>
27 
28 #include <pilz_msgs/IsBrakeTestRequired.h>
29 #include <pilz_msgs/IsBrakeTestRequiredResult.h>
30 
31 #include <prbt_hardware_support/ModbusMsgInStamped.h>
33 #include <prbt_hardware_support/SendBrakeTestResult.h>
35 
36 namespace prbt_hardware_support
37 {
38 
39 using TWriteModbusRegister = std::function<bool(const uint16_t&, const RegCont&)>;
40 
46 {
47 public:
48  ModbusAdapterBrakeTest(TWriteModbusRegister&& write_modbus_register_func,
49  const ModbusApiSpec& read_api_spec,
50  const ModbusApiSpec& write_api_spec);
51 
52 public:
59  void modbusMsgCallback(const ModbusMsgInStampedConstPtr& msg_raw);
60 
66  bool isBrakeTestRequired(pilz_msgs::IsBrakeTestRequired::Request&,
67  pilz_msgs::IsBrakeTestRequired::Response& res);
68 
72  bool sendBrakeTestResult(SendBrakeTestResult::Request& req,
73  SendBrakeTestResult::Response& res);
74 
75 private:
76  using TBrakeTestRequired = pilz_msgs::IsBrakeTestRequiredResult::_value_type;
77  void updateBrakeTestRequiredState(TBrakeTestRequired brake_test_required);
78 
79 private:
80  using TRegIdx = uint16_t;
81  using TRegIdxCont = std::map<std::string, TRegIdx>;
86  static TRegIdxCont getRegisters(const ModbusApiSpec &write_api_spec);
87 
89  static const TRegIdxCont::mapped_type& getMinRegisterIdx(const TRegIdxCont& reg_idx_cont);
91  static const TRegIdxCont::mapped_type& getMaxRegisterIdx(const TRegIdxCont& reg_idx_cont);
92 
95  static unsigned long getRegisterBlockSize(const TRegIdxCont& reg_idx_cont);
96 
97 private:
99 
101  TBrakeTestRequired brake_test_required_ {pilz_msgs::IsBrakeTestRequiredResult::UNKNOWN};
102 
106 
107  const TRegIdx reg_start_idx_ {getMinRegisterIdx(reg_idx_cont_)};
108  const RegCont::size_type reg_block_size_ {getRegisterBlockSize(reg_idx_cont_)};
109 
111 };
112 
113 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
114 
115 inline bool ModbusAdapterBrakeTest::isBrakeTestRequired(pilz_msgs::IsBrakeTestRequired::Request& /*req*/,
116  pilz_msgs::IsBrakeTestRequired::Response& res)
117 {
118  res.result.value = brake_test_required_;
119  return true;
120 }
121 
122 // The following is excluded because lambda functions are not marked properly with gcc-7
123 // see https://github.com/gcc-mirror/gcc/commit/7de708f
124 // LCOV_EXCL_START
125 inline const ModbusAdapterBrakeTest::TRegIdxCont::mapped_type& ModbusAdapterBrakeTest::getMinRegisterIdx(const TRegIdxCont& reg_idx_cont)
126 {
127  return std::min_element(
128  reg_idx_cont.cbegin(), reg_idx_cont.cend(),
129  [](const TRegIdxCont::value_type& l, const TRegIdxCont::value_type& r) -> bool { return l.second < r.second; })
130  ->second;
131 }
132 
133 inline const ModbusAdapterBrakeTest::TRegIdxCont::mapped_type& ModbusAdapterBrakeTest::getMaxRegisterIdx(const TRegIdxCont& reg_idx_cont)
134 {
135  return std::max_element(
136  reg_idx_cont.cbegin(), reg_idx_cont.cend(),
137  [](const TRegIdxCont::value_type& l, const TRegIdxCont::value_type& r) -> bool { return l.second < r.second; })
138  ->second;
139 }
140 // LCVO EXCL_STOP
141 
143 {
144  return static_cast<unsigned long>(std::abs(getMaxRegisterIdx(reg_idx_cont) - getMinRegisterIdx(reg_idx_cont)) + 1);
145 }
146 
147 } // namespace prbt_hardware_support
148 #endif // MODBUS_ADAPTER_BRAKE_TEST_H
static const TRegIdxCont::mapped_type & getMaxRegisterIdx(const TRegIdxCont &reg_idx_cont)
ModbusAdapterBrakeTest(TWriteModbusRegister &&write_modbus_register_func, const ModbusApiSpec &read_api_spec, const ModbusApiSpec &write_api_spec)
TBrakeTestRequired brake_test_required_
Store the current state of whether a brake test is required.
bool sendBrakeTestResult(SendBrakeTestResult::Request &req, SendBrakeTestResult::Response &res)
Sends the brake test result data to the modbus client.
bool isBrakeTestRequired(pilz_msgs::IsBrakeTestRequired::Request &, pilz_msgs::IsBrakeTestRequired::Response &res)
Stores the brake test required flag and initializes the brake test service, the first time the functi...
static const TRegIdxCont::mapped_type & getMinRegisterIdx(const TRegIdxCont &reg_idx_cont)
void updateBrakeTestRequiredState(TBrakeTestRequired brake_test_required)
void modbusMsgCallback(const ModbusMsgInStampedConstPtr &msg_raw)
Called whenever a new modbus message arrives.
static unsigned long getRegisterBlockSize(const TRegIdxCont &reg_idx_cont)
std::function< bool(const uint16_t &, const RegCont &)> TWriteModbusRegister
Listens to the modbus_read topic and publishes a message informing about a required brake test...
static TRegIdxCont getRegisters(const ModbusApiSpec &write_api_spec)
pilz_msgs::IsBrakeTestRequiredResult::_value_type TBrakeTestRequired
Specifies the meaning of the holding registers.


prbt_hardware_support
Author(s):
autogenerated on Tue Feb 25 2020 03:19:31