ros2_canopen  master
C++ ROS CANopen Library
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 
21 using namespace ros2_canopen::node_interfaces;
22 
23 template <class NODETYPE>
25 : ros2_canopen::node_interfaces::NodeCanopenBaseDriver<NODETYPE>(node)
26 {
27 }
28 
29 template <class NODETYPE>
30 void NodeCanopenProxyDriver<NODETYPE>::init(bool called_from_base)
31 {
32  RCLCPP_ERROR(this->node_->get_logger(), "Not init implemented.");
33 }
34 
35 template <>
36 void 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 
72 template <>
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 
111 template <class NODETYPE>
112 void 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 
170 template <class NODETYPE>
171 void 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 
180 template <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 
195 template <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 
211 template <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 
219 template <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 
232 template <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 
240 template <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 
253 template <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 
263 template <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 
294 template <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 
303 template <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 
336 template <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