ros2_canopen master
C++ ROS CANopen Library
Loading...
Searching...
No Matches
node_canopen_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_DRIVER_HPP_
16#define NODE_CANOPEN_DRIVER_HPP_
17
18#include <yaml-cpp/yaml.h>
19#include <atomic>
20#include <lely/coapp/driver.hpp>
21#include <map>
22#include <memory>
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>
36#include <vector>
40#include "canopen_interfaces/srv/co_node.hpp"
41
42using namespace rclcpp;
43namespace ros2_canopen
44{
45namespace node_interfaces
46{
58template <class NODETYPE>
60{
61 static_assert(
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");
65
66protected:
67 NODETYPE * node_;
68
69 std::shared_ptr<lely::ev::Executor> exec_;
70 std::shared_ptr<lely::canopen::AsyncMaster> master_;
71 std::shared_ptr<lely::canopen::BasicDriver> driver_;
72
73 std::chrono::milliseconds non_transmit_timeout_;
74 YAML::Node config_;
75 uint8_t node_id_;
76 std::string container_name_;
77 std::string eds_;
78 std::string bin_;
79
80 rclcpp::CallbackGroup::SharedPtr client_cbg_;
81 rclcpp::CallbackGroup::SharedPtr timer_cbg_;
82
83 std::atomic<bool> master_set_;
84 std::atomic<bool> initialised_;
85 std::atomic<bool> configured_;
86 std::atomic<bool> activated_;
87
88public:
89 NodeCanopenDriver(NODETYPE * node)
90 : master_set_(false), initialised_(false), configured_(false), activated_(false)
91 {
92 node_ = node;
93 }
94
102 virtual void set_master(
103 std::shared_ptr<lely::ev::Executor> exec, std::shared_ptr<lely::canopen::AsyncMaster> master)
104 {
105 RCLCPP_DEBUG(node_->get_logger(), "set_master_start");
106 if (!configured_.load())
107 {
108 throw DriverException("Set Master: driver is not configured");
109 }
110 if (activated_.load())
111 {
112 throw DriverException("Set Master: driver is not activated");
113 }
114 this->exec_ = exec;
115 this->master_ = master;
116 this->master_set_.store(true);
117 RCLCPP_DEBUG(node_->get_logger(), "set_master_end");
118 }
119
127 void init()
128 {
129 RCLCPP_DEBUG(node_->get_logger(), "init_start");
130 if (configured_.load())
131 {
132 throw DriverException("Init: Driver is already configured");
133 }
134 if (activated_.load())
135 {
136 throw DriverException("Init: Driver is already activated");
137 }
138 client_cbg_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
139 timer_cbg_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
140
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", "");
145 this->init(true);
146 this->initialised_.store(true);
147 RCLCPP_DEBUG(node_->get_logger(), "init_end");
148 }
149
162 virtual void init(bool called_from_base) {}
163
176 {
177 RCLCPP_DEBUG(node_->get_logger(), "configure_start");
178 if (!initialised_.load())
179 {
180 throw DriverException("Configure: driver is not initialised");
181 }
182 if (configured_.load())
183 {
184 throw DriverException("Configure: driver is already configured");
185 }
186 if (activated_.load())
187 {
188 throw DriverException("Configure: driver is already activated");
189 }
190 int non_transmit_timeout;
191 std::string config;
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");
206 }
214 virtual void configure(bool called_from_base) {}
215
223 void activate()
224 {
225 RCLCPP_DEBUG(node_->get_logger(), "activate_start");
226 if (!master_set_.load())
227 {
228 throw DriverException("Activate: master is not set");
229 }
230 if (!initialised_.load())
231 {
232 throw DriverException("Activate: driver is not initialised");
233 }
234 if (!configured_.load())
235 {
236 throw DriverException("Activate: driver is not configured");
237 }
238 if (activated_.load())
239 {
240 throw DriverException("Activate: driver is already activated");
241 }
242 this->add_to_master();
243 this->activate(true);
244 this->activated_.store(true);
245 RCLCPP_DEBUG(node_->get_logger(), "activate_end");
246 }
247
256 virtual void activate(bool called_from_base) {}
257
266 {
267 RCLCPP_DEBUG(node_->get_logger(), "deactivate_start");
268 if (!master_set_.load())
269 {
270 throw DriverException("Activate: master is not set");
271 }
272 if (!initialised_.load())
273 {
274 throw DriverException("Deactivate: driver is not initialised");
275 }
276 if (!configured_.load())
277 {
278 throw DriverException("Deactivate: driver is not configured");
279 }
280 if (!activated_.load())
281 {
282 throw DriverException("Deactivate: driver is not activated");
283 }
284 this->activated_.store(false);
285 this->remove_from_master();
286 this->deactivate(true);
287 RCLCPP_DEBUG(node_->get_logger(), "deactivate_end");
288 }
296 virtual void deactivate(bool called_from_base) {}
297
305 void cleanup()
306 {
307 if (!initialised_.load())
308 {
309 throw DriverException("Cleanup: driver is not initialised");
310 }
311 if (!configured_.load())
312 {
313 throw DriverException("Cleanup: driver is not configured");
314 }
315 if (activated_.load())
316 {
317 throw DriverException("Cleanup: driver is still activated");
318 }
319 this->cleanup(true);
320 this->configured_.store(false);
321 }
322
330 virtual void cleanup(bool called_from_base)
331 {
332 RCLCPP_INFO(this->node_->get_logger(), "Cleanup");
333 this->exec_.reset();
334 this->master_.reset();
335 this->master_set_.store(false);
336 }
337
344 void shutdown()
345 {
346 RCLCPP_DEBUG(this->node_->get_logger(), "Shutting down.");
347 if (this->activated_)
348 {
349 this->deactivate();
350 }
351 if (this->configured_)
352 {
353 this->cleanup();
354 }
355 shutdown(true);
356 this->master_set_.store(false);
357 this->initialised_.store(false);
358 this->configured_.store(false);
359 this->activated_.store(false);
360 }
368 virtual void shutdown(bool called_from_base) {}
369
370 virtual void demand_set_master();
371
372protected:
377 virtual void add_to_master() { throw DriverException("Add to master not implemented."); }
378
383 virtual void remove_from_master()
384 {
385 throw DriverException("Remove from master not implemented.");
386 }
387};
388} // namespace node_interfaces
389} // namespace ros2_canopen
390
391#endif
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:67
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