ros2_canopen master
C++ ROS CANopen Library
Loading...
Searching...
No Matches
node_canopen_proxy_driver_impl.hpp
Go to the documentation of this file.
1// Copyright 2022 Christoph Hellmann Santos
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7// http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
15#ifndef NODE_CANOPEN_PROXY_DRIVER_IMPL_HPP_
16#define NODE_CANOPEN_PROXY_DRIVER_IMPL_HPP_
17
20
21using namespace ros2_canopen::node_interfaces;
22
23template <class NODETYPE>
25: ros2_canopen::node_interfaces::NodeCanopenBaseDriver<NODETYPE>(node)
26{
27}
28
29template <class NODETYPE>
30void NodeCanopenProxyDriver<NODETYPE>::init(bool called_from_base)
31{
32 RCLCPP_ERROR(this->node_->get_logger(), "Not init implemented.");
33}
34
35template <>
36void NodeCanopenProxyDriver<rclcpp::Node>::init(bool called_from_base)
37{
38 nmt_state_publisher = this->node_->create_publisher<std_msgs::msg::String>(
39 std::string(this->node_->get_name()).append("/nmt_state").c_str(), 10);
40 tpdo_subscriber = this->node_->create_subscription<canopen_interfaces::msg::COData>(
41 std::string(this->node_->get_name()).append("/tpdo").c_str(), 10,
42 std::bind(&NodeCanopenProxyDriver<rclcpp::Node>::on_tpdo, this, std::placeholders::_1));
43
44 rpdo_publisher = this->node_->create_publisher<canopen_interfaces::msg::COData>(
45 std::string(this->node_->get_name()).append("/rpdo").c_str(), 10);
46
47 nmt_state_reset_service = this->node_->create_service<std_srvs::srv::Trigger>(
48 std::string(this->node_->get_name()).append("/nmt_reset_node").c_str(),
49 std::bind(
51 std::placeholders::_2));
52
53 nmt_state_start_service = this->node_->create_service<std_srvs::srv::Trigger>(
54 std::string(this->node_->get_name()).append("/nmt_start_node").c_str(),
55 std::bind(
57 std::placeholders::_2));
58
59 sdo_read_service = this->node_->create_service<canopen_interfaces::srv::CORead>(
60 std::string(this->node_->get_name()).append("/sdo_read").c_str(),
61 std::bind(
62 &NodeCanopenProxyDriver<rclcpp::Node>::on_sdo_read, this, std::placeholders::_1,
63 std::placeholders::_2));
64
65 sdo_write_service = this->node_->create_service<canopen_interfaces::srv::COWrite>(
66 std::string(this->node_->get_name()).append("/sdo_write").c_str(),
67 std::bind(
68 &NodeCanopenProxyDriver<rclcpp::Node>::on_sdo_write, this, std::placeholders::_1,
69 std::placeholders::_2));
70}
71
72template <>
74{
75 nmt_state_publisher = this->node_->create_publisher<std_msgs::msg::String>(
76 std::string(this->node_->get_name()).append("/nmt_state").c_str(), 10);
77 tpdo_subscriber = this->node_->create_subscription<canopen_interfaces::msg::COData>(
78 std::string(this->node_->get_name()).append("/tpdo").c_str(), 10,
79 std::bind(
81 std::placeholders::_1));
82
83 rpdo_publisher = this->node_->create_publisher<canopen_interfaces::msg::COData>(
84 std::string(this->node_->get_name()).append("/rpdo").c_str(), 10);
85
86 nmt_state_reset_service = this->node_->create_service<std_srvs::srv::Trigger>(
87 std::string(this->node_->get_name()).append("/nmt_reset_node").c_str(),
88 std::bind(
90 std::placeholders::_1, std::placeholders::_2));
91
92 nmt_state_start_service = this->node_->create_service<std_srvs::srv::Trigger>(
93 std::string(this->node_->get_name()).append("/nmt_start_node").c_str(),
94 std::bind(
96 std::placeholders::_1, std::placeholders::_2));
97
98 sdo_read_service = this->node_->create_service<canopen_interfaces::srv::CORead>(
99 std::string(this->node_->get_name()).append("/sdo_read").c_str(),
100 std::bind(
102 std::placeholders::_1, std::placeholders::_2));
103
104 sdo_write_service = this->node_->create_service<canopen_interfaces::srv::COWrite>(
105 std::string(this->node_->get_name()).append("/sdo_write").c_str(),
106 std::bind(
108 std::placeholders::_1, std::placeholders::_2));
109}
110
111template <class NODETYPE>
112void NodeCanopenProxyDriver<NODETYPE>::on_nmt(canopen::NmtState nmt_state)
113{
114 if (this->activated_.load())
115 {
116 auto message = std_msgs::msg::String();
117
118 switch (nmt_state)
119 {
120 case canopen::NmtState::BOOTUP:
121 message.data = "BOOTUP";
122 this->diagnostic_collector_->updateAll(
123 diagnostic_msgs::msg::DiagnosticStatus::OK, "NMT bootup", "NMT", "BOOTUP");
124 break;
125 case canopen::NmtState::PREOP:
126 message.data = "PREOP";
127 this->diagnostic_collector_->updateAll(
128 diagnostic_msgs::msg::DiagnosticStatus::OK, "NMT preop", "NMT", "PREOP");
129 break;
130 case canopen::NmtState::RESET_COMM:
131 message.data = "RESET_COMM";
132 this->diagnostic_collector_->updateAll(
133 diagnostic_msgs::msg::DiagnosticStatus::WARN, "NMT reset comm", "NMT", "RESET_COMM");
134 break;
135 case canopen::NmtState::RESET_NODE:
136 message.data = "RESET_NODE";
137 this->diagnostic_collector_->updateAll(
138 diagnostic_msgs::msg::DiagnosticStatus::WARN, "NMT reset node", "NMT", "RESET_NODE");
139 break;
140 case canopen::NmtState::START:
141 message.data = "START";
142 this->diagnostic_collector_->updateAll(
143 diagnostic_msgs::msg::DiagnosticStatus::OK, "NMT start", "NMT", "START");
144 break;
145 case canopen::NmtState::STOP:
146 message.data = "STOP";
147 this->diagnostic_collector_->updateAll(
148 diagnostic_msgs::msg::DiagnosticStatus::OK, "NMT stop", "NMT", "STOP");
149 break;
150 case canopen::NmtState::TOGGLE:
151 message.data = "TOGGLE";
152 this->diagnostic_collector_->updateAll(
153 diagnostic_msgs::msg::DiagnosticStatus::OK, "NMT toggle", "NMT", "TOGGLE");
154 break;
155 default:
156 RCLCPP_ERROR(this->node_->get_logger(), "Unknown NMT State.");
157 message.data = "ERROR";
158 this->diagnostic_collector_->updateAll(
159 diagnostic_msgs::msg::DiagnosticStatus::ERROR, "NMT unknown state", "NMT", "ERROR");
160 break;
161 }
162 RCLCPP_INFO(
163 this->node_->get_logger(), "Slave 0x%X: Switched NMT state to %s",
164 this->lely_driver_->get_id(), message.data.c_str());
165
166 nmt_state_publisher->publish(message);
167 }
168}
169
170template <class NODETYPE>
171void NodeCanopenProxyDriver<NODETYPE>::on_tpdo(const canopen_interfaces::msg::COData::SharedPtr msg)
172{
173 ros2_canopen::COData data = {msg->index, msg->subindex, msg->data};
174 if (!tpdo_transmit(data))
175 {
176 RCLCPP_ERROR(this->node_->get_logger(), "Could transmit PDO because driver not activated.");
177 }
178}
179
180template <class NODETYPE>
182{
183 if (this->activated_.load())
184 {
185 RCLCPP_INFO(
186 this->node_->get_logger(), "Node ID 0x%X: Transmit PDO index %x, subindex %hhu, data %d",
187 this->lely_driver_->get_id(), data.index_, data.subindex_,
188 data.data_); // ToDo: Remove or make debug
189 this->lely_driver_->tpdo_transmit(data);
190 return true;
191 }
192 return false;
193}
194
195template <class NODETYPE>
197{
198 if (this->activated_.load())
199 {
200 // RCLCPP_INFO(
201 // this->node_->get_logger(), "Node ID 0x%X: Received PDO index %#04x, subindex %hhu, data
202 // %x", this->lely_driver_->get_id(), d.index_, d.subindex_, d.data_);
203 auto message = canopen_interfaces::msg::COData();
204 message.index = d.index_;
205 message.subindex = d.subindex_;
206 message.data = d.data_;
207 rpdo_publisher->publish(message);
208 }
209}
210
211template <class NODETYPE>
213 const std_srvs::srv::Trigger::Request::SharedPtr request,
214 std_srvs::srv::Trigger::Response::SharedPtr response)
215{
216 response->success = reset_node_nmt_command();
217}
218
219template <class NODETYPE>
221{
222 if (this->activated_.load())
223 {
224 this->lely_driver_->nmt_command(canopen::NmtCommand::RESET_NODE);
225 return true;
226 }
227 RCLCPP_ERROR(
228 this->node_->get_logger(), "Could not reset device via NMT because driver not activated.");
229 return false;
230}
231
232template <class NODETYPE>
234 const std_srvs::srv::Trigger::Request::SharedPtr request,
235 std_srvs::srv::Trigger::Response::SharedPtr response)
236{
237 response->success = start_node_nmt_command();
238}
239
240template <class NODETYPE>
242{
243 if (this->activated_.load())
244 {
245 this->lely_driver_->nmt_command(canopen::NmtCommand::START);
246 return true;
247 }
248 RCLCPP_ERROR(
249 this->node_->get_logger(), "Could not start device via NMT because driver not activated.");
250 return false;
251}
252
253template <class NODETYPE>
255 const canopen_interfaces::srv::CORead::Request::SharedPtr request,
256 canopen_interfaces::srv::CORead::Response::SharedPtr response)
257{
258 ros2_canopen::COData data = {request->index, request->subindex, 0U};
259 response->success = sdo_read(data);
260 response->data = data.data_;
261}
262
263template <class NODETYPE>
265{
266 if (this->activated_.load())
267 {
268 RCLCPP_INFO(
269 this->node_->get_logger(), "Slave 0x%X: SDO Read Call index=0x%X subindex=%hhu",
270 this->lely_driver_->get_id(), data.index_, data.subindex_);
271
272 // Only allow one SDO request concurrently
273 std::scoped_lock<std::mutex> lk(sdo_mtex);
274 // Send read request
275 auto f = this->lely_driver_->async_sdo_read(data);
276 // Wait for response
277 f.wait();
278 // Process response
279 try
280 {
281 data.data_ = f.get().data_;
282 }
283 catch (std::exception & e)
284 {
285 RCLCPP_ERROR(this->node_->get_logger(), e.what());
286 return false;
287 }
288 return true;
289 }
290 RCLCPP_ERROR(this->node_->get_logger(), "Could not read from SDO because driver not activated.");
291 return false;
292}
293
294template <class NODETYPE>
296 const canopen_interfaces::srv::COWrite::Request::SharedPtr request,
297 canopen_interfaces::srv::COWrite::Response::SharedPtr response)
298{
299 ros2_canopen::COData data = {request->index, request->subindex, request->data};
300 response->success = sdo_write(data);
301}
302
303template <class NODETYPE>
305{
306 if (this->activated_.load())
307 {
308 RCLCPP_INFO(
309 this->node_->get_logger(), "Slave 0x%X: SDO Write Call index=0x%X subindex=%hhu data=%u",
310 this->lely_driver_->get_id(), data.index_, data.subindex_, data.data_);
311
312 // Only allow one SDO request concurrently
313 std::scoped_lock<std::mutex> lk(sdo_mtex);
314
315 // Send write request
316 auto f = this->lely_driver_->async_sdo_write(data);
317 // Wait for request to complete
318 f.wait();
319
320 // Process response
321 try
322 {
323 return f.get();
324 }
325 catch (std::exception & e)
326 {
327 RCLCPP_ERROR(this->node_->get_logger(), e.what());
328 return false;
329 }
330 return true;
331 }
332 RCLCPP_ERROR(this->node_->get_logger(), "Could not write to SDO because driver not activated.");
333 return false;
334}
335
336template <class NODETYPE>
338 diagnostic_updater::DiagnosticStatusWrapper & stat)
339{
340 stat.summary(this->diagnostic_collector_->getLevel(), this->diagnostic_collector_->getMessage());
341 stat.add("device_state", this->diagnostic_collector_->getValue("DEVICE"));
342 stat.add("nmt_state", this->diagnostic_collector_->getValue("NMT"));
343 stat.add("emcy_state", this->diagnostic_collector_->getValue("EMCY"));
344}
345
346#endif
Definition node_canopen_base_driver.hpp:34
void init()
Initialise the driver.
Definition node_canopen_driver.hpp:127
Definition node_canopen_proxy_driver.hpp:26
void on_sdo_read(const canopen_interfaces::srv::CORead::Request::SharedPtr request, canopen_interfaces::srv::CORead::Response::SharedPtr response)
Definition node_canopen_proxy_driver_impl.hpp:254
virtual bool sdo_write(COData &data)
Definition node_canopen_proxy_driver_impl.hpp:304
void on_nmt_state_reset(const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response)
Definition node_canopen_proxy_driver_impl.hpp:212
virtual void diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper &stat) override
Definition node_canopen_proxy_driver_impl.hpp:337
void on_sdo_write(const canopen_interfaces::srv::COWrite::Request::SharedPtr request, canopen_interfaces::srv::COWrite::Response::SharedPtr response)
Definition node_canopen_proxy_driver_impl.hpp:295
void on_nmt_state_start(const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response)
Definition node_canopen_proxy_driver_impl.hpp:233
virtual void on_rpdo(COData data) override
Definition node_canopen_proxy_driver_impl.hpp:196
NodeCanopenProxyDriver(NODETYPE *node)
Definition node_canopen_proxy_driver_impl.hpp:24
virtual bool reset_node_nmt_command()
Definition node_canopen_proxy_driver_impl.hpp:220
virtual void on_tpdo(const canopen_interfaces::msg::COData::SharedPtr msg)
Definition node_canopen_proxy_driver_impl.hpp:171
virtual bool tpdo_transmit(COData &data)
Definition node_canopen_proxy_driver_impl.hpp:181
virtual bool start_node_nmt_command()
Definition node_canopen_proxy_driver_impl.hpp:241
virtual void on_nmt(canopen::NmtState nmt_state) override
Definition node_canopen_proxy_driver_impl.hpp:112
virtual bool sdo_read(COData &data)
Definition node_canopen_proxy_driver_impl.hpp:264
Definition node_canopen_driver.hpp:46
Definition configuration_manager.hpp:28
Definition exchange.hpp:26
uint32_t data_
Definition exchange.hpp:30
uint8_t subindex_
Definition exchange.hpp:29
uint16_t index_
Definition exchange.hpp:28