15 #ifndef NODE_CANOPEN_DRIVER_HPP_
16 #define NODE_CANOPEN_DRIVER_HPP_
18 #include <yaml-cpp/yaml.h>
20 #include <lely/coapp/driver.hpp>
23 #include <rclcpp/node_interfaces/node_base_interface.hpp>
24 #include <rclcpp/node_interfaces/node_clock_interface.hpp>
25 #include <rclcpp/node_interfaces/node_graph_interface.hpp>
26 #include <rclcpp/node_interfaces/node_logging_interface.hpp>
27 #include <rclcpp/node_interfaces/node_parameters_interface.hpp>
28 #include <rclcpp/node_interfaces/node_services_interface.hpp>
29 #include <rclcpp/node_interfaces/node_time_source_interface.hpp>
30 #include <rclcpp/node_interfaces/node_timers_interface.hpp>
31 #include <rclcpp/node_interfaces/node_topics_interface.hpp>
32 #include <rclcpp/node_interfaces/node_waitables_interface.hpp>
33 #include <rclcpp/rclcpp.hpp>
34 #include <rclcpp_lifecycle/lifecycle_node.hpp>
35 #include <type_traits>
40 #include "canopen_interfaces/srv/co_node.hpp"
42 using namespace rclcpp;
45 namespace node_interfaces
58 template <
class NODETYPE>
62 std::is_base_of<rclcpp::Node, NODETYPE>::value ||
63 std::is_base_of<rclcpp_lifecycle::LifecycleNode, NODETYPE>::value,
64 "NODETYPE must derive from rclcpp::Node or rclcpp_lifecycle::LifecycleNode");
69 std::shared_ptr<lely::ev::Executor>
exec_;
70 std::shared_ptr<lely::canopen::AsyncMaster>
master_;
71 std::shared_ptr<lely::canopen::BasicDriver>
driver_;
90 : master_set_(false), initialised_(false), configured_(false), activated_(false)
103 std::shared_ptr<lely::ev::Executor> exec, std::shared_ptr<lely::canopen::AsyncMaster> master)
105 RCLCPP_DEBUG(node_->get_logger(),
"set_master_start");
106 if (!configured_.load())
110 if (activated_.load())
115 this->master_ = master;
116 this->master_set_.store(
true);
117 RCLCPP_DEBUG(node_->get_logger(),
"set_master_end");
129 RCLCPP_DEBUG(node_->get_logger(),
"init_start");
130 if (configured_.load())
134 if (activated_.load())
138 client_cbg_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
139 timer_cbg_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
141 node_->declare_parameter(
"container_name",
"");
142 node_->declare_parameter(
"node_id", 0);
143 node_->declare_parameter(
"non_transmit_timeout", 100);
144 node_->declare_parameter(
"config",
"");
146 this->initialised_.store(
true);
147 RCLCPP_DEBUG(node_->get_logger(),
"init_end");
162 virtual void init(
bool called_from_base) {}
177 RCLCPP_DEBUG(node_->get_logger(),
"configure_start");
178 if (!initialised_.load())
182 if (configured_.load())
186 if (activated_.load())
190 int non_transmit_timeout;
192 node_->get_parameter(
"container_name", container_name_);
193 node_->get_parameter(
"non_transmit_timeout", non_transmit_timeout);
194 node_->get_parameter(
"node_id", this->node_id_);
195 node_->get_parameter(
"config", config);
196 this->config_ = YAML::Load(config);
197 this->non_transmit_timeout_ = std::chrono::milliseconds(non_transmit_timeout);
198 auto path = this->config_[
"dcf_path"].as<std::string>();
199 auto dcf = this->config_[
"dcf"].as<std::string>();
200 auto name = this->node_->get_name();
201 eds_ = path +
"/" + dcf;
202 bin_ = path +
"/" + name +
".bin";
203 this->configure(
true);
204 this->configured_.store(
true);
205 RCLCPP_DEBUG(node_->get_logger(),
"configure_end");
225 RCLCPP_DEBUG(node_->get_logger(),
"activate_start");
226 if (!master_set_.load())
230 if (!initialised_.load())
234 if (!configured_.load())
238 if (activated_.load())
242 this->add_to_master();
243 this->activate(
true);
244 this->activated_.store(
true);
245 RCLCPP_DEBUG(node_->get_logger(),
"activate_end");
267 RCLCPP_DEBUG(node_->get_logger(),
"deactivate_start");
268 if (!master_set_.load())
272 if (!initialised_.load())
276 if (!configured_.load())
280 if (!activated_.load())
284 this->activated_.store(
false);
285 this->remove_from_master();
286 this->deactivate(
true);
287 RCLCPP_DEBUG(node_->get_logger(),
"deactivate_end");
307 if (!initialised_.load())
311 if (!configured_.load())
315 if (activated_.load())
320 this->configured_.store(
false);
332 RCLCPP_INFO(this->node_->get_logger(),
"Cleanup");
334 this->master_.reset();
335 this->master_set_.store(
false);
346 RCLCPP_DEBUG(this->node_->get_logger(),
"Shutting down.");
347 if (this->activated_)
351 if (this->configured_)
356 this->master_set_.store(
false);
357 this->initialised_.store(
false);
358 this->configured_.store(
false);
359 this->activated_.store(
false);
Driver Exception.
Definition: driver_error.hpp:31
Node Canopen Driver Interface.
Definition: node_canopen_driver_interface.hpp:33
Node Canopen Driver.
Definition: node_canopen_driver.hpp:60
std::string bin_
Definition: node_canopen_driver.hpp:78
virtual void remove_from_master()
Remove the driver from master.
Definition: node_canopen_driver.hpp:383
std::atomic< bool > initialised_
Definition: node_canopen_driver.hpp:84
void shutdown()
Shutdown the driver.
Definition: node_canopen_driver.hpp:344
virtual void init(bool called_from_base)
Initialises the driver.
Definition: node_canopen_driver.hpp:162
std::atomic< bool > master_set_
Definition: node_canopen_driver.hpp:83
std::string eds_
Definition: node_canopen_driver.hpp:77
void deactivate()
Deactivate the driver.
Definition: node_canopen_driver.hpp:265
std::atomic< bool > configured_
Definition: node_canopen_driver.hpp:85
void cleanup()
Cleanup the driver.
Definition: node_canopen_driver.hpp:305
void activate()
Activate the driver.
Definition: node_canopen_driver.hpp:223
std::string container_name_
Definition: node_canopen_driver.hpp:76
NODETYPE * node_
Definition: node_canopen_driver.hpp:64
virtual void demand_set_master()
Demand set Master.
uint8_t node_id_
Definition: node_canopen_driver.hpp:75
std::atomic< bool > activated_
Definition: node_canopen_driver.hpp:86
virtual void set_master(std::shared_ptr< lely::ev::Executor > exec, std::shared_ptr< lely::canopen::AsyncMaster > master)
Set Master.
Definition: node_canopen_driver.hpp:102
rclcpp::CallbackGroup::SharedPtr client_cbg_
Definition: node_canopen_driver.hpp:80
virtual void deactivate(bool called_from_base)
Deactivates the driver.
Definition: node_canopen_driver.hpp:296
std::shared_ptr< lely::canopen::BasicDriver > driver_
Definition: node_canopen_driver.hpp:71
void init()
Initialise the driver.
Definition: node_canopen_driver.hpp:127
void configure()
Configure the driver.
Definition: node_canopen_driver.hpp:175
virtual void cleanup(bool called_from_base)
Cleanup the driver.
Definition: node_canopen_driver.hpp:330
rclcpp::CallbackGroup::SharedPtr timer_cbg_
Definition: node_canopen_driver.hpp:81
virtual void add_to_master()
Add the driver to master.
Definition: node_canopen_driver.hpp:377
virtual void shutdown(bool called_from_base)
Shuts down the driver.
Definition: node_canopen_driver.hpp:368
virtual void activate(bool called_from_base)
Activates the driver.
Definition: node_canopen_driver.hpp:256
std::chrono::milliseconds non_transmit_timeout_
Definition: node_canopen_driver.hpp:73
std::shared_ptr< lely::canopen::AsyncMaster > master_
Definition: node_canopen_driver.hpp:70
YAML::Node config_
Definition: node_canopen_driver.hpp:74
NodeCanopenDriver(NODETYPE *node)
Definition: node_canopen_driver.hpp:89
std::shared_ptr< lely::ev::Executor > exec_
Definition: node_canopen_driver.hpp:69
virtual void configure(bool called_from_base)
Configure the driver.
Definition: node_canopen_driver.hpp:214
Definition: configuration_manager.hpp:28