ros2_canopen  master
C++ ROS CANopen Library
node_canopen_base_driver.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_BASE_DRIVER
16 #define NODE_CANOPEN_BASE_DRIVER
17 
21 #include "std_msgs/msg/string.hpp"
22 #include "std_srvs/srv/trigger.hpp"
23 
24 #include "canopen_interfaces/msg/co_data.hpp"
25 #include "canopen_interfaces/srv/co_read.hpp"
26 #include "canopen_interfaces/srv/co_write.hpp"
27 
28 namespace ros2_canopen
29 {
30 namespace node_interfaces
31 {
32 template <class NODETYPE>
33 class NodeCanopenBaseDriver : public NodeCanopenDriver<NODETYPE>
34 {
35  static_assert(
36  std::is_base_of<rclcpp::Node, NODETYPE>::value ||
37  std::is_base_of<rclcpp_lifecycle::LifecycleNode, NODETYPE>::value,
38  "NODETYPE must derive from rclcpp::Node or rclcpp_lifecycle::LifecycleNode");
39 
40 protected:
44  std::mutex driver_mutex_;
45  std::shared_ptr<ros2_canopen::LelyDriverBridge> lely_driver_;
46  uint32_t period_ms_;
48  bool polling_;
49 
50  // nmt state callback
51  std::function<void(canopen::NmtState, uint8_t)> nmt_state_cb_;
52  // rpdo callback
53  std::function<void(COData, uint8_t)> rpdo_cb_;
54  // emcy callback
55  std::function<void(COEmcy, uint8_t)> emcy_cb_;
56 
57  std::shared_ptr<ros2_canopen::SafeQueue<ros2_canopen::COEmcy>> emcy_queue_;
58  std::shared_ptr<ros2_canopen::SafeQueue<ros2_canopen::COData>> rpdo_queue_;
59  rclcpp::TimerBase::SharedPtr poll_timer_;
60 
61  // Diagnostic components
62  std::atomic<bool> diagnostic_enabled_;
64  std::shared_ptr<diagnostic_updater::Updater> diagnostic_updater_;
65  std::shared_ptr<DiagnosticsCollector> diagnostic_collector_;
66 
67  virtual void poll_timer_callback();
68  void nmt_listener();
69  virtual void on_nmt(canopen::NmtState nmt_state);
70  void rdpo_listener();
71  virtual void on_rpdo(COData data);
72  void emcy_listener();
73  virtual void on_emcy(COEmcy emcy);
74  virtual void diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat);
75 
76 public:
77  NodeCanopenBaseDriver(NODETYPE * node);
78 
80  {
81  if (nmt_state_publisher_thread_.joinable())
82  {
84  }
85  if (rpdo_publisher_thread_.joinable())
86  {
88  }
89  }
90 
91  virtual void init(bool called_from_base);
92 
93  virtual void configure(bool called_from_base);
94 
95  virtual void activate(bool called_from_base);
96 
97  virtual void deactivate(bool called_from_base);
98 
99  virtual void cleanup(bool called_from_base);
100 
101  virtual void shutdown(bool called_from_base);
102 
103  virtual void add_to_master();
104 
105  virtual void remove_from_master();
106 
112  void register_nmt_state_cb(std::function<void(canopen::NmtState, uint8_t)> nmt_state_cb)
113  {
114  nmt_state_cb_ = std::move(nmt_state_cb);
115  }
116 
122  void register_rpdo_cb(std::function<void(COData, uint8_t)> rpdo_cb)
123  {
124  rpdo_cb_ = std::move(rpdo_cb);
125  }
126 
132  void register_emcy_cb(std::function<void(COEmcy, uint8_t)> emcy_cb)
133  {
134  emcy_cb_ = std::move(emcy_cb);
135  }
136 };
139 } // namespace node_interfaces
140 } // namespace ros2_canopen
141 
142 #endif
Definition: node_canopen_base_driver.hpp:34
uint32_t period_ms_
Definition: node_canopen_base_driver.hpp:46
virtual ~NodeCanopenBaseDriver()
Definition: node_canopen_base_driver.hpp:79
rclcpp::TimerBase::SharedPtr poll_timer_
Definition: node_canopen_base_driver.hpp:59
uint32_t diagnostic_period_ms_
Definition: node_canopen_base_driver.hpp:63
void register_emcy_cb(std::function< void(COEmcy, uint8_t)> emcy_cb)
Register a callback for EMCY.
Definition: node_canopen_base_driver.hpp:132
void rdpo_listener()
Definition: node_canopen_base_driver_impl.hpp:376
virtual void add_to_master()
Add the driver to master.
Definition: node_canopen_base_driver_impl.hpp:245
std::shared_ptr< DiagnosticsCollector > diagnostic_collector_
Definition: node_canopen_base_driver.hpp:65
void nmt_listener()
Definition: node_canopen_base_driver_impl.hpp:318
virtual void configure(bool called_from_base)
Configure the driver.
std::thread nmt_state_publisher_thread_
Definition: node_canopen_base_driver.hpp:38
std::shared_ptr< diagnostic_updater::Updater > diagnostic_updater_
Definition: node_canopen_base_driver.hpp:64
virtual void on_rpdo(COData data)
Definition: node_canopen_base_driver_impl.hpp:352
void register_nmt_state_cb(std::function< void(canopen::NmtState, uint8_t)> nmt_state_cb)
Register a callback for NMT state change.
Definition: node_canopen_base_driver.hpp:112
virtual void poll_timer_callback()
Definition: node_canopen_base_driver_impl.hpp:403
std::atomic< bool > diagnostic_enabled_
Definition: node_canopen_base_driver.hpp:62
std::shared_ptr< ros2_canopen::SafeQueue< ros2_canopen::COEmcy > > emcy_queue_
Definition: node_canopen_base_driver.hpp:57
void register_rpdo_cb(std::function< void(COData, uint8_t)> rpdo_cb)
Register a callback for RPDO.
Definition: node_canopen_base_driver.hpp:122
NodeCanopenBaseDriver(NODETYPE *node)
Definition: node_canopen_base_driver_impl.hpp:23
std::function< void(COData, uint8_t)> rpdo_cb_
Definition: node_canopen_base_driver.hpp:53
std::thread emcy_publisher_thread_
Definition: node_canopen_base_driver.hpp:43
std::mutex driver_mutex_
Definition: node_canopen_base_driver.hpp:44
virtual void diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper &stat)
Definition: node_canopen_base_driver_impl.hpp:478
int sdo_timeout_ms_
Definition: node_canopen_base_driver.hpp:47
virtual void remove_from_master()
Remove the driver from master.
Definition: node_canopen_base_driver_impl.hpp:293
void emcy_listener()
Definition: node_canopen_base_driver_impl.hpp:450
std::thread rpdo_publisher_thread_
Definition: node_canopen_base_driver.hpp:42
std::shared_ptr< ros2_canopen::SafeQueue< ros2_canopen::COData > > rpdo_queue_
Definition: node_canopen_base_driver.hpp:58
std::function< void(canopen::NmtState, uint8_t)> nmt_state_cb_
Definition: node_canopen_base_driver.hpp:51
bool polling_
Definition: node_canopen_base_driver.hpp:48
std::function< void(COEmcy, uint8_t)> emcy_cb_
Definition: node_canopen_base_driver.hpp:55
std::shared_ptr< ros2_canopen::LelyDriverBridge > lely_driver_
Definition: node_canopen_base_driver.hpp:45
virtual void on_nmt(canopen::NmtState nmt_state)
Definition: node_canopen_base_driver_impl.hpp:347
virtual void on_emcy(COEmcy emcy)
Definition: node_canopen_base_driver_impl.hpp:357
Node Canopen Driver.
Definition: node_canopen_driver.hpp:60
void shutdown()
Shutdown the driver.
Definition: node_canopen_driver.hpp:344
void deactivate()
Deactivate the driver.
Definition: node_canopen_driver.hpp:265
void cleanup()
Cleanup the driver.
Definition: node_canopen_driver.hpp:305
void activate()
Activate the driver.
Definition: node_canopen_driver.hpp:223
void init()
Initialise the driver.
Definition: node_canopen_driver.hpp:127
NodeCanopenBaseDriver< rclcpp_lifecycle::LifecycleNode > NCBDLifecycleNode
Definition: node_canopen_base_driver.hpp:138
NodeCanopenBaseDriver< rclcpp::Node > NCBDNode
Definition: node_canopen_base_driver.hpp:137
Definition: configuration_manager.hpp:28
Definition: exchange.hpp:26
Definition: exchange.hpp:34