modbus_adapter_brake_test.cpp
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  */
18 
19 #include <sstream>
20 #include <algorithm>
21 
25 
26 namespace prbt_hardware_support
27 {
28 using namespace modbus_api::v3;
29 
31  const ModbusApiSpec& read_api_spec, const ModbusApiSpec& write_api_spec)
32  : api_spec_(read_api_spec)
33  , reg_idx_cont_(getRegisters(write_api_spec))
34  , reg_start_idx_(getMinRegisterIdx(reg_idx_cont_))
35  , reg_block_size_(getRegisterBlockSize(reg_idx_cont_))
36  , write_modbus_register_func_(write_modbus_register_func)
37 {
38 }
39 
41 {
42  TRegIdxCont reg_idx_cont;
43 
45  {
46  throw ModbusAdapterBrakeTestException("Failed to read API spec for BRAKETEST_PERFORMED");
47  }
50 
52  {
53  throw ModbusAdapterBrakeTestException("Failed to read API spec for BRAKETEST_RESULT");
54  }
55  reg_idx_cont[modbus_api_spec::BRAKETEST_RESULT] =
57 
58  if (abs(reg_idx_cont.at(modbus_api_spec::BRAKETEST_PERFORMED) - reg_idx_cont.at(modbus_api_spec::BRAKETEST_RESULT)) !=
59  1)
60  {
61  std::ostringstream os;
62  os << "Registers of BRAKETEST_PERFORMED and BRAKETEST_RESULT need to be 1 apart";
63  os << " (distance: " << abs(reg_idx_cont.cbegin()->second - reg_idx_cont.cend()->second) << ")";
64  // Both registers need to be one apart, so that we can write them in one cycle
65  throw ModbusAdapterBrakeTestException(os.str());
66  }
67 
68  return reg_idx_cont;
69 }
70 
71 void ModbusAdapterBrakeTest::modbusMsgCallback(const ModbusMsgInStampedConstPtr& msg_raw)
72 {
74 
75  if (msg.isDisconnect())
76  {
77  return;
78  }
79 
80  try
81  {
82  msg.checkStructuralIntegrity();
83  }
84  catch (const ModbusMsgWrapperException& ex)
85  {
86  ROS_ERROR_STREAM(ex.what());
87  return;
88  }
89 
90  if (msg.getVersion() != MODBUS_API_VERSION_REQUIRED)
91  {
92  std::ostringstream os;
93  os << "Received Modbus message of unsupported API Version: " << msg.getVersion()
94  << ", required Version: " << MODBUS_API_VERSION_REQUIRED;
95  os << "\n";
96  os << "Can not determine from Modbus message if brake-test is required.";
97  ROS_ERROR_STREAM(os.str());
98  return;
99  }
100 
101  updateBrakeTestRequiredState(msg.getBrakeTestRequirementStatus());
102 }
103 
105 {
106  TBrakeTestRequired last_brake_test_flag{ brake_test_required_ };
107  brake_test_required_ = brake_test_required;
108  if (brake_test_required_ == pilz_msgs::IsBrakeTestRequiredResult::REQUIRED &&
109  last_brake_test_flag != pilz_msgs::IsBrakeTestRequiredResult::REQUIRED)
110  {
111  ROS_INFO("Brake Test required.");
112  }
113 }
114 
115 bool ModbusAdapterBrakeTest::sendBrakeTestResult(SendBrakeTestResult::Request& req, SendBrakeTestResult::Response& res)
116 {
118  {
119  res.error_msg = "No callback available to send brake test result to FS control";
120  res.success = false;
121  return true;
122  }
123 
124  RegCont reg_cont(reg_block_size_, 0);
125  // Note: The FS controller needs a positive edge, so we first reset the registers
129 
131  {
132  res.error_msg = "Resetting of modus registers failed";
133  res.success = false;
134  return true;
135  }
136 
138  if (req.result)
139  {
141  }
143  {
144  res.error_msg = "Sending of brake test result to FS control failed";
145  res.success = false;
146  return true;
147  }
148 
149  res.success = true;
150  return true;
151 }
152 
153 } // namespace prbt_hardware_support
msg
static const std::string BRAKETEST_PERFORMED
std::vector< uint16_t > RegCont
Convenience data type defining the data type for a collection of registers.
unsigned short getRegisterDefinition(const std::string &key) const
ModbusAdapterBrakeTest(TWriteModbusRegister &&write_modbus_register_func, const ModbusApiSpec &read_api_spec, const ModbusApiSpec &write_api_spec)
static const std::string BRAKETEST_RESULT
static constexpr uint16_t MODBUS_BRAKE_TEST_PERFORMED
TBrakeTestRequired brake_test_required_
Store the current state of whether a brake test is required.
Exception thrown by the ModbusAdapterBrakeTest class.
static constexpr uint16_t MODBUS_API_VERSION_REQUIRED
bool sendBrakeTestResult(SendBrakeTestResult::Request &req, SendBrakeTestResult::Response &res)
Sends the brake test result data to the modbus client.
#define ROS_INFO(...)
void updateBrakeTestRequiredState(TBrakeTestRequired brake_test_required)
static constexpr uint16_t MODBUS_BRAKE_TEST_PASSED
Wrapper class to add semantic to a raw ModbusMsgInStamped.
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
void modbusMsgCallback(const ModbusMsgInStampedConstPtr &msg_raw)
Called whenever a new modbus message arrives.
static constexpr uint16_t MODBUS_BRAKE_TEST_NOT_PERFORMED
Expection thrown upon construction of ModbusMsgWrapperBase of the message does not contain the requir...
std::function< bool(const uint16_t &, const RegCont &)> TWriteModbusRegister
#define ROS_ERROR_STREAM(args)
static TRegIdxCont getRegisters(const ModbusApiSpec &write_api_spec)
static constexpr uint16_t MODBUS_BRAKE_TEST_NOT_PASSED
bool hasRegisterDefinition(const std::string &key) const
pilz_msgs::IsBrakeTestRequiredResult::_value_type TBrakeTestRequired
Specifies the meaning of the holding registers.


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