15 #ifndef NODE_CANOPEN_MASTER_HPP_
16 #define NODE_CANOPEN_MASTER_HPP_
18 #include <yaml-cpp/yaml.h>
20 #include <lely/coapp/master.hpp>
21 #include <lely/coapp/slave.hpp>
22 #include <lely/ev/exec.hpp>
23 #include <lely/ev/loop.hpp>
24 #include <lely/io2/linux/can.hpp>
25 #include <lely/io2/posix/poll.hpp>
26 #include <lely/io2/sys/io.hpp>
27 #include <lely/io2/sys/sigset.hpp>
28 #include <lely/io2/sys/timer.hpp>
29 #include <rclcpp/rclcpp.hpp>
30 #include <rclcpp_lifecycle/lifecycle_node.hpp>
37 namespace node_interfaces
50 template <
class NODETYPE>
54 std::is_base_of<rclcpp::Node, NODETYPE>::value ||
55 std::is_base_of<rclcpp_lifecycle::LifecycleNode, NODETYPE>::value,
56 "NODETYPE must derive from rclcpp::Node or rclcpp_lifecycle::LifecycleNode");
66 std::shared_ptr<lely::canopen::AsyncMaster>
master_;
67 std::shared_ptr<lely::ev::Executor>
exec_;
70 std::unique_ptr<lely::io::Context>
ctx_;
71 std::unique_ptr<lely::io::Poll>
poll_;
72 std::unique_ptr<lely::ev::Loop>
loop_;
73 std::unique_ptr<lely::io::Timer>
timer_;
74 std::unique_ptr<lely::io::CanController>
ctrl_;
75 std::unique_ptr<lely::io::CanChannel>
chan_;
76 std::unique_ptr<lely::io::SignalSet>
sigset_;
105 RCLCPP_DEBUG(
node_->get_logger(),
"init_start");
114 client_cbg_ =
node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
115 timer_cbg_ =
node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
117 node_->declare_parameter(
"container_name",
"");
118 node_->declare_parameter(
"master_dcf",
"");
119 node_->declare_parameter(
"master_bin",
"");
120 node_->declare_parameter(
"can_interface_name",
"vcan0");
121 node_->declare_parameter(
"node_id", 0);
122 node_->declare_parameter(
"non_transmit_timeout", 100);
123 node_->declare_parameter(
"config",
"");
125 this->initialised_.store(
true);
126 RCLCPP_DEBUG(
node_->get_logger(),
"init_end");
128 virtual void init(
bool called_from_base) {}
152 int non_transmit_timeout;
160 node_->get_parameter(
"non_transmit_timeout", non_transmit_timeout);
161 node_->get_parameter(
"config", config);
163 this->config_ = YAML::Load(config);
164 this->non_transmit_timeout_ = std::chrono::milliseconds(non_transmit_timeout);
167 this->configured_.store(
true);
172 std::optional<int> timeout;
175 timeout = this->config_[
"boot_timeout"].as<
int>();
180 this->node_->get_logger(),
181 "No timeout parameter found in config file. Using default value of 100ms.");
183 this->timeout_ = timeout.value_or(2000);
185 this->node_->get_logger(),
"Master boot timeout set to " << this->timeout_ <<
"ms.");
197 RCLCPP_DEBUG(this->node_->get_logger(),
"NodeCanopenMaster activate start");
211 io_guard_ = std::make_unique<lely::io::IoGuard>();
212 ctx_ = std::make_unique<lely::io::Context>();
213 poll_ = std::make_unique<lely::io::Poll>(*
ctx_);
214 loop_ = std::make_unique<lely::ev::Loop>(
poll_->get_poll());
216 exec_ = std::make_shared<lely::ev::Executor>(
loop_->get_executor());
217 timer_ = std::make_unique<lely::io::Timer>(*
poll_, *
exec_, CLOCK_MONOTONIC);
227 this->master_set_.store(
true);
228 this->master_->Reset();
229 this->spinner_ = std::thread(
236 catch (
const std::exception & e)
238 RCLCPP_INFO(this->node_->get_logger(), e.what());
240 RCLCPP_INFO(this->node_->get_logger(),
"Canopen master loop stopped");
242 this->activated_.store(
true);
243 RCLCPP_DEBUG(this->node_->get_logger(),
"NodeCanopenMaster activate end");
280 RCLCPP_INFO(
node_->get_logger(),
"Lely Core Context Shutdown");
283 this->spinner_.join();
286 this->activated_.store(
false);
330 this->configured_.store(
false);
331 this->master_set_.store(
false);
333 virtual void cleanup(
bool called_from_base) {}
343 RCLCPP_DEBUG(this->node_->get_logger(),
"Shutting down.");
344 if (this->activated_)
349 if (this->configured_)
355 this->master_set_.store(
false);
356 this->initialised_.store(
false);
357 this->configured_.store(
false);
358 this->activated_.store(
false);
367 virtual std::shared_ptr<lely::canopen::AsyncMaster>
get_master()
Master Exception.
Definition: master_error.hpp:30
Node Canopen Master Interface.
Definition: node_canopen_master_interface.hpp:34
Node Canopen Master.
Definition: node_canopen_master.hpp:52
YAML::Node config_
Definition: node_canopen_master.hpp:81
void shutdown() override
Shutdown the driver.
Definition: node_canopen_master.hpp:341
std::unique_ptr< lely::ev::Loop > loop_
Definition: node_canopen_master.hpp:72
rclcpp::CallbackGroup::SharedPtr client_cbg_
Definition: node_canopen_master.hpp:78
virtual void deactivate(bool called_from_base)
Deactivate hook for derived classes.
Definition: node_canopen_master.hpp:297
void configure() override
Configure the driver.
Definition: node_canopen_master.hpp:137
virtual void init(bool called_from_base)
Definition: node_canopen_master.hpp:128
virtual void shutdown(bool called_from_base)
Definition: node_canopen_master.hpp:360
std::string can_interface_name_
Definition: node_canopen_master.hpp:87
std::string master_bin_
Definition: node_canopen_master.hpp:86
std::shared_ptr< lely::ev::Executor > exec_
Definition: node_canopen_master.hpp:67
std::unique_ptr< lely::io::CanChannel > chan_
Definition: node_canopen_master.hpp:75
void init() override
Initialise Master.
Definition: node_canopen_master.hpp:103
virtual std::shared_ptr< lely::canopen::AsyncMaster > get_master()
Get the master object.
Definition: node_canopen_master.hpp:367
virtual void activate(bool called_from_base)
Activate hook for derived classes.
Definition: node_canopen_master.hpp:254
std::atomic< bool > master_set_
Definition: node_canopen_master.hpp:64
std::unique_ptr< lely::io::Context > ctx_
Definition: node_canopen_master.hpp:70
std::unique_ptr< lely::io::IoGuard > io_guard_
Definition: node_canopen_master.hpp:69
std::unique_ptr< lely::io::Poll > poll_
Definition: node_canopen_master.hpp:71
std::string container_name_
Definition: node_canopen_master.hpp:84
virtual std::shared_ptr< lely::ev::Executor > get_executor()
Get the executor object.
Definition: node_canopen_master.hpp:380
std::chrono::milliseconds non_transmit_timeout_
Definition: node_canopen_master.hpp:83
NODETYPE * node_
Definition: node_canopen_master.hpp:56
virtual void configure(bool called_from_base)
Definition: node_canopen_master.hpp:170
std::unique_ptr< lely::io::CanController > ctrl_
Definition: node_canopen_master.hpp:74
std::shared_ptr< lely::canopen::AsyncMaster > master_
Definition: node_canopen_master.hpp:66
void deactivate() override
Deactivate the driver.
Definition: node_canopen_master.hpp:263
void cleanup() override
Cleanup the driver.
Definition: node_canopen_master.hpp:306
rclcpp::CallbackGroup::SharedPtr timer_cbg_
Definition: node_canopen_master.hpp:79
std::unique_ptr< lely::io::Timer > timer_
Definition: node_canopen_master.hpp:73
std::atomic< bool > activated_
Definition: node_canopen_master.hpp:63
virtual void cleanup(bool called_from_base)
Definition: node_canopen_master.hpp:333
void activate() override
Activate the driver.
Definition: node_canopen_master.hpp:195
std::atomic< bool > initialised_
Definition: node_canopen_master.hpp:61
std::thread spinner_
Definition: node_canopen_master.hpp:90
std::unique_ptr< lely::io::SignalSet > sigset_
Definition: node_canopen_master.hpp:76
NodeCanopenMaster(NODETYPE *node)
Definition: node_canopen_master.hpp:93
uint32_t timeout_
Definition: node_canopen_master.hpp:88
std::string master_dcf_
Definition: node_canopen_master.hpp:85
uint8_t node_id_
Definition: node_canopen_master.hpp:82
std::atomic< bool > configured_
Definition: node_canopen_master.hpp:62
Definition: configuration_manager.hpp:28