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 
24 
25 namespace prbt_hardware_support
26 {
27 
28 static constexpr unsigned int MODBUS_API_VERSION_REQUIRED {2};
29 static constexpr unsigned int BRAKE_TEST_PERFORMED {1};
30 
32  const ModbusApiSpec& read_api_spec,
33  const ModbusApiSpec& write_api_spec)
34  : api_spec_(read_api_spec)
35  , reg_idx_cont_(getRegisters(write_api_spec))
36  , reg_start_idx_(getMinRegisterIdx(reg_idx_cont_))
37  , reg_block_size_(getRegisterBlockSize(reg_idx_cont_))
38  , write_modbus_register_func_(write_modbus_register_func)
39 {
40 
41 }
42 
44 {
45  TRegIdxCont reg_idx_cont;
46 
48  {
49  throw ModbusAdapterBrakeTestException("Failed to read API spec for BRAKETEST_PERFORMED");
50  }
53 
55  {
56  throw ModbusAdapterBrakeTestException("Failed to read API spec for BRAKETEST_RESULT");
57  }
58  reg_idx_cont[modbus_api_spec::BRAKETEST_RESULT] =
60 
61  if(abs(reg_idx_cont.at(modbus_api_spec::BRAKETEST_PERFORMED) - reg_idx_cont.at(modbus_api_spec::BRAKETEST_RESULT)) != 1)
62  {
63  std::ostringstream os;
64  os << "Registers of BRAKETEST_PERFORMED and BRAKETEST_RESULT need to be 1 apart";
65  os << " (distance: " << abs(reg_idx_cont.cbegin()->second - reg_idx_cont.cend()->second) << ")";
66  // Both registers need to be one apart, so that we can write them in one cycle
67  throw ModbusAdapterBrakeTestException(os.str());
68  }
69 
70  return reg_idx_cont;
71 }
72 
73 void ModbusAdapterBrakeTest::modbusMsgCallback(const ModbusMsgInStampedConstPtr& msg_raw)
74 {
76 
77  if (msg.isDisconnect())
78  {
79  return;
80  }
81 
82  try
83  {
84  msg.checkStructuralIntegrity();
85  }
86  catch(const ModbusMsgWrapperException &ex)
87  {
88  ROS_ERROR_STREAM(ex.what());
89  return;
90  }
91 
92  if(msg.getVersion() != MODBUS_API_VERSION_REQUIRED)
93  {
94  std::ostringstream os;
95  os << "Received Modbus message of unsupported API Version: "
96  << msg.getVersion()
97  << ", required Version: " << MODBUS_API_VERSION_REQUIRED;
98  os <<"\n";
99  os << "Can not determine from Modbus message if brake-test is required.";
100  ROS_ERROR_STREAM(os.str());
101  return;
102  }
103 
104  updateBrakeTestRequiredState(msg.getBrakeTestRequirementStatus());
105 }
106 
108 {
109  TBrakeTestRequired last_brake_test_flag {brake_test_required_};
110  brake_test_required_ = brake_test_required;
111  if(brake_test_required_ == pilz_msgs::IsBrakeTestRequiredResult::REQUIRED
112  && last_brake_test_flag != pilz_msgs::IsBrakeTestRequiredResult::REQUIRED)
113  {
114  ROS_INFO("Brake Test required.");
115  }
116 }
117 
118 bool ModbusAdapterBrakeTest::sendBrakeTestResult(SendBrakeTestResult::Request& req,
119  SendBrakeTestResult::Response& res)
120 {
122  {
123  res.error_msg = "No callback available to send brake test result to FS control";
124  res.success = false;
125  return true;
126  }
127 
128  RegCont reg_cont(reg_block_size_, 0);
129  // Note: The FS controller needs a positive edge, so we first reset the
130  // registers by sending 0s.
132  {
133  res.error_msg = "Resetting of modus registers failed";
134  res.success = false;
135  return true;
136  }
137 
139  reg_cont.at(reg_idx_cont_.at(modbus_api_spec::BRAKETEST_RESULT) - reg_start_idx_) = req.result;
141  {
142  res.error_msg = "Sending of brake test result to FS control failed";
143  res.success = false;
144  return true;
145  }
146 
147  res.success = true;
148  return true;
149 }
150 
151 } // 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
TBrakeTestRequired brake_test_required_
Store the current state of whether a brake test is required.
Exception thrown by the ModbusAdapterBrakeTest class.
static constexpr unsigned int BRAKE_TEST_PERFORMED
bool sendBrakeTestResult(SendBrakeTestResult::Request &req, SendBrakeTestResult::Response &res)
Sends the brake test result data to the modbus client.
#define ROS_INFO(...)
static constexpr unsigned int MODBUS_API_VERSION_REQUIRED
void updateBrakeTestRequiredState(TBrakeTestRequired brake_test_required)
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.
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)
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 Fri Feb 28 2020 03:16:57