15 #ifndef NODE_CANOPEN_BASE_DRIVER
16 #define NODE_CANOPEN_BASE_DRIVER
21 #include "std_msgs/msg/string.hpp"
22 #include "std_srvs/srv/trigger.hpp"
24 #include "canopen_interfaces/msg/co_data.hpp"
25 #include "canopen_interfaces/srv/co_read.hpp"
26 #include "canopen_interfaces/srv/co_write.hpp"
30 namespace node_interfaces
32 template <
class NODETYPE>
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");
57 std::shared_ptr<ros2_canopen::SafeQueue<ros2_canopen::COEmcy>>
emcy_queue_;
58 std::shared_ptr<ros2_canopen::SafeQueue<ros2_canopen::COData>>
rpdo_queue_;
69 virtual void on_nmt(canopen::NmtState nmt_state);
91 virtual void init(
bool called_from_base);
95 virtual void activate(
bool called_from_base);
97 virtual void deactivate(
bool called_from_base);
99 virtual void cleanup(
bool called_from_base);
101 virtual void shutdown(
bool called_from_base);
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