ros2_canopen master
C++ ROS CANopen Library
Loading...
Searching...
No Matches
node_canopen_402_driver_impl.hpp
Go to the documentation of this file.
1// Copyright 2023 Christoph Hellmann Santos
2// Vishnuprasad Prachandabhanu
3// Lovro Ivanov
4//
5// This program is free software: you can redistribute it and/or modify
6// it under the terms of the GNU General Public License as published by
7// the Free Software Foundation, either version 3 of the License, or
8// (at your option) any later version.
9//
10// This program is distributed in the hope that it will be useful,
11// but WITHOUT ANY WARRANTY; without even the implied warranty of
12// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13// GNU General Public License for more details.
14//
15// You should have received a copy of the GNU General Public License
16// along with this program. If not, see <https://www.gnu.org/licenses/>.
17//
18
19#ifndef NODE_CANOPEN_402_DRIVER_IMPL_HPP_
20#define NODE_CANOPEN_402_DRIVER_IMPL_HPP_
21
24
25#include <optional>
26
27using namespace ros2_canopen::node_interfaces;
28using namespace std::placeholders;
29
30template <class NODETYPE>
32: ros2_canopen::node_interfaces::NodeCanopenProxyDriver<NODETYPE>(node)
33{
34}
35
36template <class NODETYPE>
37void NodeCanopen402Driver<NODETYPE>::init(bool called_from_base)
38{
39 RCLCPP_ERROR(this->node_->get_logger(), "Not init implemented.");
40}
41
42template <>
43void NodeCanopen402Driver<rclcpp::Node>::init(bool called_from_base)
44{
46 publish_joint_state =
47 this->node_->create_publisher<sensor_msgs::msg::JointState>("~/joint_states", 1);
48 handle_init_service = this->node_->create_service<std_srvs::srv::Trigger>(
49 std::string(this->node_->get_name()).append("/init").c_str(),
50 std::bind(
51 &NodeCanopen402Driver<rclcpp::Node>::handle_init, this, std::placeholders::_1,
52 std::placeholders::_2));
53
54 handle_enable_service = this->node_->create_service<std_srvs::srv::Trigger>(
55 std::string(this->node_->get_name()).append("/enable").c_str(),
56 std::bind(
57 &NodeCanopen402Driver<rclcpp::Node>::handle_enable, this, std::placeholders::_1,
58 std::placeholders::_2));
59
60 handle_disable_service = this->node_->create_service<std_srvs::srv::Trigger>(
61 std::string(this->node_->get_name()).append("/disable").c_str(),
62 std::bind(
63 &NodeCanopen402Driver<rclcpp::Node>::handle_disable, this, std::placeholders::_1,
64 std::placeholders::_2));
65
66 handle_halt_service = this->node_->create_service<std_srvs::srv::Trigger>(
67 std::string(this->node_->get_name()).append("/halt").c_str(),
68 std::bind(
69 &NodeCanopen402Driver<rclcpp::Node>::handle_halt, this, std::placeholders::_1,
70 std::placeholders::_2));
71
72 handle_recover_service = this->node_->create_service<std_srvs::srv::Trigger>(
73 std::string(this->node_->get_name()).append("/recover").c_str(),
74 std::bind(
75 &NodeCanopen402Driver<rclcpp::Node>::handle_recover, this, std::placeholders::_1,
76 std::placeholders::_2));
77
78 handle_set_mode_position_service = this->node_->create_service<std_srvs::srv::Trigger>(
79 std::string(this->node_->get_name()).append("/position_mode").c_str(),
80 std::bind(
82 std::placeholders::_2));
83
84 handle_set_mode_velocity_service = this->node_->create_service<std_srvs::srv::Trigger>(
85 std::string(this->node_->get_name()).append("/velocity_mode").c_str(),
86 std::bind(
88 std::placeholders::_2));
89
90 handle_set_mode_cyclic_velocity_service = this->node_->create_service<std_srvs::srv::Trigger>(
91 std::string(this->node_->get_name()).append("/cyclic_velocity_mode").c_str(),
92 std::bind(
94 std::placeholders::_1, std::placeholders::_2));
95
96 handle_set_mode_cyclic_position_service = this->node_->create_service<std_srvs::srv::Trigger>(
97 std::string(this->node_->get_name()).append("/cyclic_position_mode").c_str(),
98 std::bind(
100 std::placeholders::_1, std::placeholders::_2));
101
102 handle_set_mode_interpolated_position_service =
103 this->node_->create_service<std_srvs::srv::Trigger>(
104 std::string(this->node_->get_name()).append("/interpolated_position_mode").c_str(),
105 std::bind(
107 std::placeholders::_1, std::placeholders::_2));
108
109 handle_set_mode_torque_service = this->node_->create_service<std_srvs::srv::Trigger>(
110 std::string(this->node_->get_name()).append("/torque_mode").c_str(),
111 std::bind(
113 std::placeholders::_2));
114
115 handle_set_mode_cyclic_torque_service = this->node_->create_service<std_srvs::srv::Trigger>(
116 std::string(this->node_->get_name()).append("/cyclic_torque_mode").c_str(),
117 std::bind(
119 std::placeholders::_1, std::placeholders::_2));
120
121 handle_set_target_service = this->node_->create_service<canopen_interfaces::srv::COTargetDouble>(
122 std::string(this->node_->get_name()).append("/target").c_str(),
123 std::bind(
124 &NodeCanopen402Driver<rclcpp::Node>::handle_set_target, this, std::placeholders::_1,
125 std::placeholders::_2));
126}
127
128template <>
130{
132 publish_joint_state =
133 this->node_->create_publisher<sensor_msgs::msg::JointState>("~/joint_states", 10);
134 handle_init_service = this->node_->create_service<std_srvs::srv::Trigger>(
135 std::string(this->node_->get_name()).append("/init").c_str(),
136 std::bind(
138 std::placeholders::_1, std::placeholders::_2));
139
140 handle_enable_service = this->node_->create_service<std_srvs::srv::Trigger>(
141 std::string(this->node_->get_name()).append("/enable").c_str(),
142 std::bind(
144 std::placeholders::_1, std::placeholders::_2));
145
146 handle_disable_service = this->node_->create_service<std_srvs::srv::Trigger>(
147 std::string(this->node_->get_name()).append("/disable").c_str(),
148 std::bind(
150 std::placeholders::_1, std::placeholders::_2));
151
152 handle_halt_service = this->node_->create_service<std_srvs::srv::Trigger>(
153 std::string(this->node_->get_name()).append("/halt").c_str(),
154 std::bind(
156 std::placeholders::_1, std::placeholders::_2));
157
158 handle_recover_service = this->node_->create_service<std_srvs::srv::Trigger>(
159 std::string(this->node_->get_name()).append("/recover").c_str(),
160 std::bind(
162 std::placeholders::_1, std::placeholders::_2));
163
164 handle_set_mode_position_service = this->node_->create_service<std_srvs::srv::Trigger>(
165 std::string(this->node_->get_name()).append("/position_mode").c_str(),
166 std::bind(
168 std::placeholders::_1, std::placeholders::_2));
169
170 handle_set_mode_velocity_service = this->node_->create_service<std_srvs::srv::Trigger>(
171 std::string(this->node_->get_name()).append("/velocity_mode").c_str(),
172 std::bind(
174 std::placeholders::_1, std::placeholders::_2));
175
176 handle_set_mode_cyclic_velocity_service = this->node_->create_service<std_srvs::srv::Trigger>(
177 std::string(this->node_->get_name()).append("/cyclic_velocity_mode").c_str(),
178 std::bind(
180 std::placeholders::_1, std::placeholders::_2));
181
182 handle_set_mode_cyclic_position_service = this->node_->create_service<std_srvs::srv::Trigger>(
183 std::string(this->node_->get_name()).append("/cyclic_position_mode").c_str(),
184 std::bind(
186 std::placeholders::_1, std::placeholders::_2));
187
188 handle_set_mode_interpolated_position_service = this->node_->create_service<
189 std_srvs::srv::Trigger>(
190 std::string(this->node_->get_name()).append("/interpolated_position_mode").c_str(),
191 std::bind(
193 this, std::placeholders::_1, std::placeholders::_2));
194
195 handle_set_mode_torque_service = this->node_->create_service<std_srvs::srv::Trigger>(
196 std::string(this->node_->get_name()).append("/torque_mode").c_str(),
197 std::bind(
199 std::placeholders::_1, std::placeholders::_2));
200
201 handle_set_mode_cyclic_torque_service = this->node_->create_service<std_srvs::srv::Trigger>(
202 std::string(this->node_->get_name()).append("/cyclic_torque_mode").c_str(),
203 std::bind(
205 std::placeholders::_1, std::placeholders::_2));
206
207 handle_set_target_service = this->node_->create_service<canopen_interfaces::srv::COTargetDouble>(
208 std::string(this->node_->get_name()).append("/target").c_str(),
209 std::bind(
211 std::placeholders::_1, std::placeholders::_2));
212}
213
214template <>
216{
218 std::optional<double> scale_pos_to_dev;
219 std::optional<double> scale_pos_from_dev;
220 std::optional<double> scale_vel_to_dev;
221 std::optional<double> scale_vel_from_dev;
222 std::optional<double> offset_pos_to_dev;
223 std::optional<double> offset_pos_from_dev;
224 std::optional<int> switching_state;
225 std::optional<int> homing_timeout_seconds;
226 try
227 {
228 scale_pos_to_dev = std::optional(this->config_["scale_pos_to_dev"].as<double>());
229 }
230 catch (...)
231 {
232 }
233 try
234 {
235 scale_pos_from_dev = std::optional(this->config_["scale_pos_from_dev"].as<double>());
236 }
237 catch (...)
238 {
239 }
240 try
241 {
242 scale_vel_to_dev = std::optional(this->config_["scale_vel_to_dev"].as<double>());
243 }
244 catch (...)
245 {
246 }
247 try
248 {
249 scale_vel_from_dev = std::optional(this->config_["scale_vel_from_dev"].as<double>());
250 }
251 catch (...)
252 {
253 }
254 try
255 {
256 offset_pos_to_dev = std::optional(this->config_["offset_pos_to_dev"].as<double>());
257 }
258 catch (...)
259 {
260 }
261 try
262 {
263 offset_pos_from_dev = std::optional(this->config_["offset_from_to_dev"].as<double>());
264 }
265 catch (...)
266 {
267 }
268 try
269 {
270 switching_state = std::optional(this->config_["switching_state"].as<int>());
271 }
272 catch (...)
273 {
274 }
275 try
276 {
277 homing_timeout_seconds = std::optional(this->config_["homing_timout_seconds"].as<int>());
278 }
279 catch (...)
280 {
281 }
282
283 // auto period = this->config_["scale_eff_to_dev"].as<double>();
284 // auto period = this->config_["scale_eff_from_dev"].as<double>();
285 scale_pos_to_dev_ = scale_pos_to_dev.value_or(1000.0);
286 scale_pos_from_dev_ = scale_pos_from_dev.value_or(0.001);
287 scale_vel_to_dev_ = scale_vel_to_dev.value_or(1000.0);
288 scale_vel_from_dev_ = scale_vel_from_dev.value_or(0.001);
289 offset_pos_to_dev_ = offset_pos_to_dev.value_or(0.0);
290 offset_pos_from_dev_ = offset_pos_from_dev.value_or(0.0);
291 switching_state_ = (ros2_canopen::State402::InternalState)switching_state.value_or(
293 homing_timeout_seconds_ = homing_timeout_seconds.value_or(10);
294 RCLCPP_INFO(
295 this->node_->get_logger(),
296 "scale_pos_to_dev_ %f\nscale_pos_from_dev_ %f\nscale_vel_to_dev_ %f\nscale_vel_from_dev_ "
297 "%f\noffset_pos_to_dev_ %f\noffset_pos_from_dev_ "
298 "%f\nhoming_timeout_seconds_ %i\n",
299 scale_pos_to_dev_, scale_pos_from_dev_, scale_vel_to_dev_, scale_vel_from_dev_,
300 offset_pos_to_dev_, offset_pos_from_dev_, homing_timeout_seconds_);
301}
302
303template <>
304void NodeCanopen402Driver<rclcpp::Node>::configure(bool called_from_base)
305{
307 std::optional<double> scale_pos_to_dev;
308 std::optional<double> scale_pos_from_dev;
309 std::optional<double> scale_vel_to_dev;
310 std::optional<double> scale_vel_from_dev;
311 std::optional<double> offset_pos_to_dev;
312 std::optional<double> offset_pos_from_dev;
313 std::optional<int> switching_state;
314 std::optional<int> homing_timeout_seconds;
315 try
316 {
317 scale_pos_to_dev = std::optional(this->config_["scale_pos_to_dev"].as<double>());
318 }
319 catch (...)
320 {
321 }
322 try
323 {
324 scale_pos_from_dev = std::optional(this->config_["scale_pos_from_dev"].as<double>());
325 }
326 catch (...)
327 {
328 }
329 try
330 {
331 scale_vel_to_dev = std::optional(this->config_["scale_vel_to_dev"].as<double>());
332 }
333 catch (...)
334 {
335 }
336 try
337 {
338 scale_vel_from_dev = std::optional(this->config_["scale_vel_from_dev"].as<double>());
339 }
340 catch (...)
341 {
342 }
343 try
344 {
345 offset_pos_to_dev = std::optional(this->config_["offset_pos_to_dev"].as<double>());
346 }
347 catch (...)
348 {
349 }
350 try
351 {
352 offset_pos_from_dev = std::optional(this->config_["offset_from_to_dev"].as<double>());
353 }
354 catch (...)
355 {
356 }
357 try
358 {
359 switching_state = std::optional(this->config_["switching_state"].as<int>());
360 }
361 catch (...)
362 {
363 }
364 try
365 {
366 homing_timeout_seconds = std::optional(this->config_["homing_timeout_seconds"].as<int>());
367 }
368 catch (...)
369 {
370 }
371
372 // auto period = this->config_["scale_eff_to_dev"].as<double>();
373 // auto period = this->config_["scale_eff_from_dev"].as<double>();
374 scale_pos_to_dev_ = scale_pos_to_dev.value_or(1000.0);
375 scale_pos_from_dev_ = scale_pos_from_dev.value_or(0.001);
376 scale_vel_to_dev_ = scale_vel_to_dev.value_or(1000.0);
377 scale_vel_from_dev_ = scale_vel_from_dev.value_or(0.001);
378 offset_pos_to_dev_ = offset_pos_to_dev.value_or(0.0);
379 offset_pos_from_dev_ = offset_pos_from_dev.value_or(0.0);
380 switching_state_ = (ros2_canopen::State402::InternalState)switching_state.value_or(
382 homing_timeout_seconds_ = homing_timeout_seconds.value_or(10);
383 RCLCPP_INFO(
384 this->node_->get_logger(),
385 "scale_pos_to_dev_ %f\nscale_pos_from_dev_ %f\nscale_vel_to_dev_ %f\nscale_vel_from_dev_ "
386 "%f\noffset_pos_to_dev_ %f\noffset_pos_from_dev_ "
387 "%f\nhoming_timeout_seconds_ %i\n",
388 scale_pos_to_dev_, scale_pos_from_dev_, scale_vel_to_dev_, scale_vel_from_dev_,
389 offset_pos_to_dev_, offset_pos_from_dev_, homing_timeout_seconds_);
390}
391
392template <class NODETYPE>
394{
396 motor_->registerDefaultModes();
397 motor_->set_diagnostic_status_msgs(this->diagnostic_collector_, this->diagnostic_enabled_);
398}
399
400template <class NODETYPE>
402{
404 timer_->cancel();
405}
406
407template <class NODETYPE>
409{
411 motor_->handleRead();
412 motor_->handleWrite();
413 publish();
414}
415
416template <class NODETYPE>
418{
419 sensor_msgs::msg::JointState js_msg;
420 js_msg.name.push_back(this->node_->get_name());
421 js_msg.position.push_back(motor_->get_position() * scale_pos_from_dev_ + offset_pos_from_dev_);
422 js_msg.velocity.push_back(motor_->get_speed() * scale_vel_from_dev_);
423 js_msg.effort.push_back(0.0);
424 publish_joint_state->publish(js_msg);
425}
426
427template <class NODETYPE>
429{
431 motor_ =
432 std::make_shared<Motor402>(this->lely_driver_, switching_state_, homing_timeout_seconds_);
433}
434
435template <class NODETYPE>
437 const std_srvs::srv::Trigger::Request::SharedPtr request,
438 std_srvs::srv::Trigger::Response::SharedPtr response)
439{
440 if (this->activated_.load())
441 {
442 bool temp = motor_->handleInit();
443 response->success = temp;
444 }
445}
446template <class NODETYPE>
448 const std_srvs::srv::Trigger::Request::SharedPtr request,
449 std_srvs::srv::Trigger::Response::SharedPtr response)
450{
451 if (this->activated_.load())
452 {
453 response->success = motor_->handleRecover();
454 }
455}
456template <class NODETYPE>
458 const std_srvs::srv::Trigger::Request::SharedPtr request,
459 std_srvs::srv::Trigger::Response::SharedPtr response)
460{
461 if (this->activated_.load())
462 {
463 response->success = motor_->handleHalt();
464 }
465}
466template <class NODETYPE>
468 const std_srvs::srv::Trigger::Request::SharedPtr request,
469 std_srvs::srv::Trigger::Response::SharedPtr response)
470{
471 response->success = set_operation_mode(MotorBase::Profiled_Position);
472}
473
474template <class NODETYPE>
476 const std_srvs::srv::Trigger::Request::SharedPtr request,
477 std_srvs::srv::Trigger::Response::SharedPtr response)
478{
479 response->success = set_operation_mode(MotorBase::Profiled_Velocity);
480}
481
482template <class NODETYPE>
484 const std_srvs::srv::Trigger::Request::SharedPtr request,
485 std_srvs::srv::Trigger::Response::SharedPtr response)
486{
487 response->success = set_operation_mode(MotorBase::Cyclic_Synchronous_Position);
488}
489
490template <class NODETYPE>
492 const std_srvs::srv::Trigger::Request::SharedPtr request,
493 std_srvs::srv::Trigger::Response::SharedPtr response)
494{
495 response->success = set_operation_mode(MotorBase::Interpolated_Position);
496}
497
498template <class NODETYPE>
500 const std_srvs::srv::Trigger::Request::SharedPtr request,
501 std_srvs::srv::Trigger::Response::SharedPtr response)
502{
503 response->success = set_operation_mode(MotorBase::Cyclic_Synchronous_Velocity);
504}
505template <class NODETYPE>
507 const std_srvs::srv::Trigger::Request::SharedPtr request,
508 std_srvs::srv::Trigger::Response::SharedPtr response)
509{
510 response->success = set_operation_mode(MotorBase::Profiled_Torque);
511}
512
513template <class NODETYPE>
515 const std_srvs::srv::Trigger::Request::SharedPtr request,
516 std_srvs::srv::Trigger::Response::SharedPtr response)
517{
518 response->success = set_operation_mode(MotorBase::Cyclic_Synchronous_Torque);
519}
520
521template <class NODETYPE>
523 const canopen_interfaces::srv::COTargetDouble::Request::SharedPtr request,
524 canopen_interfaces::srv::COTargetDouble::Response::SharedPtr response)
525{
526 if (this->activated_.load())
527 {
528 auto mode = motor_->getMode();
529 double target;
530 if (
533 {
534 target = request->target * scale_pos_to_dev_ + offset_pos_to_dev_;
535 }
536 else if (
537 (mode == MotorBase::Velocity) or (mode == MotorBase::Profiled_Velocity) or
539 {
540 target = request->target * scale_vel_to_dev_;
541 }
542 else
543 {
544 target = request->target;
545 }
546
547 response->success = motor_->setTarget(target);
548 }
549}
550
551template <class NODETYPE>
553 const std_srvs::srv::Trigger::Request::SharedPtr request,
554 std_srvs::srv::Trigger::Response::SharedPtr response)
555{
556 if (this->activated_.load())
557 {
558 bool temp = motor_->handleDisable();
559 response->success = temp;
560 }
561}
562
563template <class NODETYPE>
565 const std_srvs::srv::Trigger::Request::SharedPtr request,
566 std_srvs::srv::Trigger::Response::SharedPtr response)
567{
568 if (this->activated_.load())
569 {
570 bool temp = motor_->handleEnable();
571 response->success = temp;
572 }
573}
574
575template <class NODETYPE>
577{
578 if (this->activated_.load())
579 {
580 bool temp = motor_->handleInit();
581 return temp;
582 }
583 else
584 {
585 RCLCPP_INFO(this->node_->get_logger(), "Initialisation failed.");
586 return false;
587 }
588}
589
590template <class NODETYPE>
592{
593 if (this->activated_.load())
594 {
595 return motor_->handleRecover();
596 }
597 else
598 {
599 return false;
600 }
601}
602
603template <class NODETYPE>
605{
606 if (this->activated_.load())
607 {
608 return motor_->handleHalt();
609 }
610 else
611 {
612 return false;
613 }
614}
615
616template <class NODETYPE>
618{
619 if (this->activated_.load())
620 {
621 if (motor_->getMode() != mode)
622 {
623 return motor_->enterModeAndWait(mode);
624 }
625 else
626 {
627 return false;
628 }
629 }
630 return false;
631}
632
633template <class NODETYPE>
635{
636 if (this->activated_.load())
637 {
638 auto mode = motor_->getMode();
639 double scaled_target;
640 if (
643 {
644 scaled_target = target * scale_pos_to_dev_ + offset_pos_to_dev_;
645 }
646 else if (
647 (mode == MotorBase::Velocity) or (mode == MotorBase::Profiled_Velocity) or
649 {
650 scaled_target = target * scale_vel_to_dev_;
651 }
652 else
653 {
654 scaled_target = target;
655 }
656 // RCLCPP_INFO(this->node_->get_logger(), "Scaled target %f", scaled_target);
657 return motor_->setTarget(scaled_target);
658 }
659 else
660 {
661 return false;
662 }
663}
664
665template <class NODETYPE>
667 diagnostic_updater::DiagnosticStatusWrapper & stat)
668{
669 this->motor_->handleDiag();
670
671 stat.summary(this->diagnostic_collector_->getLevel(), this->diagnostic_collector_->getMessage());
672 stat.add("device_state", this->diagnostic_collector_->getValue("DEVICE"));
673 stat.add("nmt_state", this->diagnostic_collector_->getValue("NMT"));
674 stat.add("emcy_state", this->diagnostic_collector_->getValue("EMCY"));
675 stat.add("cia402_mode", this->diagnostic_collector_->getValue("cia402_mode"));
676 stat.add("cia402_state", this->diagnostic_collector_->getValue("cia402_state"));
677}
678
679#endif
@ Profiled_Velocity
Definition base.hpp:42
@ Cyclic_Synchronous_Velocity
Definition base.hpp:48
@ Cyclic_Synchronous_Torque
Definition base.hpp:49
@ Cyclic_Synchronous_Position
Definition base.hpp:47
@ Profiled_Position
Definition base.hpp:40
@ Profiled_Torque
Definition base.hpp:43
@ Interpolated_Position
Definition base.hpp:46
@ Velocity
Definition base.hpp:41
InternalState
Definition state.hpp:50
@ Operation_Enable
Definition state.hpp:57
Definition node_canopen_402_driver.hpp:35
void handle_set_mode_cyclic_velocity(const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response)
Service Callback to set cyclic velocity mode.
Definition node_canopen_402_driver_impl.hpp:499
void handle_halt(const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response)
Service Callback to halt device.
Definition node_canopen_402_driver_impl.hpp:457
void handle_enable(const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response)
Service Callback to enable device.
Definition node_canopen_402_driver_impl.hpp:564
void handle_init(const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response)
Service Callback to initialise device.
Definition node_canopen_402_driver_impl.hpp:436
bool set_target(double target)
Method to set target.
Definition node_canopen_402_driver_impl.hpp:634
void handle_set_target(const canopen_interfaces::srv::COTargetDouble::Request::SharedPtr request, canopen_interfaces::srv::COTargetDouble::Response::SharedPtr response)
Service Callback to set target.
Definition node_canopen_402_driver_impl.hpp:522
void handle_set_mode_cyclic_torque(const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response)
Service Callback to set cyclic torque mode.
Definition node_canopen_402_driver_impl.hpp:514
void handle_set_mode_velocity(const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response)
Service Callback to set profiled velocity mode.
Definition node_canopen_402_driver_impl.hpp:475
void handle_set_mode_interpolated_position(const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response)
Service Callback to set interpolated position mode.
Definition node_canopen_402_driver_impl.hpp:491
void handle_set_mode_torque(const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response)
Service Callback to set profiled torque mode.
Definition node_canopen_402_driver_impl.hpp:506
void handle_set_mode_cyclic_position(const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response)
Service Callback to set cyclic position mode.
Definition node_canopen_402_driver_impl.hpp:483
virtual void poll_timer_callback() override
Definition node_canopen_402_driver_impl.hpp:408
void handle_set_mode_position(const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response)
Service Callback to set profiled position mode.
Definition node_canopen_402_driver_impl.hpp:467
void handle_recover(const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response)
Service Callback to recover device.
Definition node_canopen_402_driver_impl.hpp:447
void diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper &stat) override
Definition node_canopen_402_driver_impl.hpp:666
bool halt_motor()
Method to halt device.
Definition node_canopen_402_driver_impl.hpp:604
bool init_motor()
Method to initialise device.
Definition node_canopen_402_driver_impl.hpp:576
bool recover_motor()
Method to recover device.
Definition node_canopen_402_driver_impl.hpp:591
NodeCanopen402Driver(NODETYPE *node)
Definition node_canopen_402_driver_impl.hpp:31
void publish()
Definition node_canopen_402_driver_impl.hpp:417
bool set_operation_mode(uint16_t mode)
Service Callback to set operation mode.
Definition node_canopen_402_driver_impl.hpp:617
void handle_disable(const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response)
Service Callback to disable device.
Definition node_canopen_402_driver_impl.hpp:552
virtual void add_to_master() override
Add the driver to master.
Definition node_canopen_402_driver_impl.hpp:428
virtual void poll_timer_callback()
Definition node_canopen_base_driver_impl.hpp:450
void deactivate()
Deactivate the driver.
Definition node_canopen_driver.hpp:265
void activate()
Activate the driver.
Definition node_canopen_driver.hpp:223
void init()
Initialise the driver.
Definition node_canopen_driver.hpp:127
void configure()
Configure the driver.
Definition node_canopen_driver.hpp:175
virtual void add_to_master()
Add the driver to master.
Definition node_canopen_driver.hpp:377
Definition node_canopen_proxy_driver.hpp:26
Definition node_canopen_driver.hpp:46
Definition configuration_manager.hpp:28