ros2_canopen master
C++ ROS CANopen Library
Loading...
Searching...
No Matches
node_canopen_master.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_MASTER_HPP_
16#define NODE_CANOPEN_MASTER_HPP_
17
18#include <yaml-cpp/yaml.h>
19#include <atomic>
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>
31#include <thread>
34
35namespace ros2_canopen
36{
37namespace node_interfaces
38{
50template <class NODETYPE>
52{
53 static_assert(
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");
57
58protected:
59 NODETYPE * node_;
60
61 std::atomic<bool> initialised_;
62 std::atomic<bool> configured_;
63 std::atomic<bool> activated_;
64 std::atomic<bool> master_set_;
65
66 std::shared_ptr<lely::canopen::AsyncMaster> master_;
67 std::shared_ptr<lely::ev::Executor> exec_;
68
69 std::unique_ptr<lely::io::IoGuard> io_guard_;
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_;
77
78 rclcpp::CallbackGroup::SharedPtr client_cbg_;
79 rclcpp::CallbackGroup::SharedPtr timer_cbg_;
80
81 YAML::Node config_;
82 uint8_t node_id_;
83 std::chrono::milliseconds non_transmit_timeout_;
84 std::string container_name_;
85 std::string master_dcf_;
86 std::string master_bin_;
88 uint32_t timeout_;
89
90 std::thread spinner_;
91
92public:
93 NodeCanopenMaster(NODETYPE * node)
94 : initialised_(false), configured_(false), activated_(false), master_set_(false)
95 {
96 node_ = node;
97 }
98
103 void init() override
104 {
105 RCLCPP_DEBUG(node_->get_logger(), "init_start");
106 if (configured_.load())
107 {
108 throw MasterException("Init: Master is already configured.");
109 }
110 if (activated_.load())
111 {
112 throw MasterException("Init: Master is already activated.");
113 }
114 client_cbg_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
115 timer_cbg_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
116
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", "");
124 this->init(true);
125 this->initialised_.store(true);
126 RCLCPP_DEBUG(node_->get_logger(), "init_end");
127 }
128 virtual void init(bool called_from_base) {}
129
137 void configure() override
138 {
139 if (!initialised_.load())
140 {
141 throw MasterException("Configure: Master is not initialised.");
142 }
143 if (configured_.load())
144 {
145 throw MasterException("Configure: Master is already configured.");
146 }
147 if (activated_.load())
148 {
149 throw MasterException("Configure: Master is already activated.");
150 }
151
152 int non_transmit_timeout;
153 std::string config;
154
155 node_->get_parameter("container_name", container_name_);
156 node_->get_parameter("master_dcf", master_dcf_);
157 node_->get_parameter("master_bin", master_bin_);
158 node_->get_parameter("can_interface_name", can_interface_name_);
159 node_->get_parameter("node_id", node_id_);
160 node_->get_parameter("non_transmit_timeout", non_transmit_timeout);
161 node_->get_parameter("config", config);
162
163 this->config_ = YAML::Load(config);
164 this->non_transmit_timeout_ = std::chrono::milliseconds(non_transmit_timeout);
165
166 this->configure(true);
167 this->configured_.store(true);
168 }
169
170 virtual void configure(bool called_from_base)
171 {
172 std::optional<int> timeout;
173 try
174 {
175 timeout = this->config_["boot_timeout"].as<int>();
176 }
177 catch (...)
178 {
179 RCLCPP_WARN(
180 this->node_->get_logger(),
181 "No timeout parameter found in config file. Using default value of 100ms.");
182 }
183 this->timeout_ = timeout.value_or(2000);
184 RCLCPP_INFO_STREAM(
185 this->node_->get_logger(), "Master boot timeout set to " << this->timeout_ << "ms.");
186 }
187
195 void activate() override
196 {
197 RCLCPP_DEBUG(this->node_->get_logger(), "NodeCanopenMaster activate start");
198 if (!initialised_.load())
199 {
200 throw MasterException("Activate: master is not initialised");
201 }
202 if (!configured_.load())
203 {
204 throw MasterException("Activate: master is not configured");
205 }
206 if (activated_.load())
207 {
208 throw MasterException("Activate: master is already activated");
209 }
210
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());
215
216 exec_ = std::make_shared<lely::ev::Executor>(loop_->get_executor());
217 timer_ = std::make_unique<lely::io::Timer>(*poll_, *exec_, CLOCK_MONOTONIC);
218 ctrl_ = std::make_unique<lely::io::CanController>(can_interface_name_.c_str());
219 chan_ = std::make_unique<lely::io::CanChannel>(*poll_, *exec_);
220 chan_->open(*ctrl_);
221
222 this->activate(true);
223 if (!master_)
224 {
225 throw MasterException("Activate: master not set");
226 }
227 this->master_set_.store(true);
228 this->master_->Reset();
229 this->spinner_ = std::thread(
230 [this]()
231 {
232 try
233 {
234 loop_->run();
235 }
236 catch (const std::exception & e)
237 {
238 RCLCPP_INFO(this->node_->get_logger(), e.what());
239 }
240 RCLCPP_INFO(this->node_->get_logger(), "Canopen master loop stopped");
241 });
242 this->activated_.store(true);
243 RCLCPP_DEBUG(this->node_->get_logger(), "NodeCanopenMaster activate end");
244 }
254 virtual void activate(bool called_from_base) {}
255
263 void deactivate() override
264 {
265 if (!initialised_.load())
266 {
267 throw MasterException("Deactivate: master is not initialised");
268 }
269 if (!configured_.load())
270 {
271 throw MasterException("Deactivate: master is not configured");
272 }
273 if (!activated_.load())
274 {
275 throw MasterException("Deactivate: master is not activated");
276 }
277 exec_->post(
278 [this]()
279 {
280 RCLCPP_INFO(node_->get_logger(), "Lely Core Context Shutdown");
281 ctx_->shutdown();
282 });
283 this->spinner_.join();
284
285 this->deactivate(true);
286 this->activated_.store(false);
287 }
288
297 virtual void deactivate(bool called_from_base) {}
298
306 void cleanup() override
307 {
308 if (!initialised_.load())
309 {
310 throw MasterException("Cleanup: master is not initialised.");
311 }
312 if (!configured_.load())
313 {
314 throw MasterException("Cleanup: master is not configured");
315 }
316 if (activated_.load())
317 {
318 throw MasterException("Cleanup: master is still active");
319 }
320 this->cleanup(true);
321 io_guard_.reset();
322 ctx_.reset();
323 poll_.reset();
324 loop_.reset();
325
326 exec_.reset();
327 timer_.reset();
328 ctrl_.reset();
329 chan_.reset();
330 this->configured_.store(false);
331 this->master_set_.store(false);
332 }
333 virtual void cleanup(bool called_from_base) {}
334
341 void shutdown() override
342 {
343 RCLCPP_DEBUG(this->node_->get_logger(), "Shutting down.");
344 if (this->activated_)
345 {
346 this->deactivate();
347 }
348
349 if (this->configured_)
350 {
351 this->cleanup();
352 }
353 shutdown(true);
354
355 this->master_set_.store(false);
356 this->initialised_.store(false);
357 this->configured_.store(false);
358 this->activated_.store(false);
359 }
360 virtual void shutdown(bool called_from_base) {}
361
367 virtual std::shared_ptr<lely::canopen::AsyncMaster> get_master()
368 {
369 if (!master_set_.load())
370 {
371 throw MasterException("Get Master: Master is not set.");
372 }
373 return master_;
374 }
380 virtual std::shared_ptr<lely::ev::Executor> get_executor()
381 {
382 if (!master_set_.load())
383 {
384 throw MasterException("Get Executor: master is not set");
385 }
386 return exec_;
387 }
388};
389} // namespace node_interfaces
390} // namespace ros2_canopen
391
392#endif
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 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
std::chrono::milliseconds non_transmit_timeout_
Definition node_canopen_master.hpp:83
NODETYPE * node_
Definition node_canopen_master.hpp:59
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
virtual std::shared_ptr< lely::ev::Executor > get_executor()
Get the executor object.
Definition node_canopen_master.hpp:380
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
virtual std::shared_ptr< lely::canopen::AsyncMaster > get_master()
Get the master object.
Definition node_canopen_master.hpp:367
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