19#ifndef NODE_CANOPEN_402_DRIVER_IMPL_HPP_
20#define NODE_CANOPEN_402_DRIVER_IMPL_HPP_
28using namespace std::placeholders;
30template <
class NODETYPE>
36template <
class NODETYPE>
39 RCLCPP_ERROR(this->node_->get_logger(),
"Not init implemented.");
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(),
52 std::placeholders::_2));
54 handle_enable_service = this->node_->create_service<std_srvs::srv::Trigger>(
55 std::string(this->node_->get_name()).append(
"/enable").c_str(),
58 std::placeholders::_2));
60 handle_disable_service = this->node_->create_service<std_srvs::srv::Trigger>(
61 std::string(this->node_->get_name()).append(
"/disable").c_str(),
64 std::placeholders::_2));
66 handle_halt_service = this->node_->create_service<std_srvs::srv::Trigger>(
67 std::string(this->node_->get_name()).append(
"/halt").c_str(),
70 std::placeholders::_2));
72 handle_recover_service = this->node_->create_service<std_srvs::srv::Trigger>(
73 std::string(this->node_->get_name()).append(
"/recover").c_str(),
76 std::placeholders::_2));
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(),
82 std::placeholders::_2));
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(),
88 std::placeholders::_2));
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(),
94 std::placeholders::_1, std::placeholders::_2));
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(),
100 std::placeholders::_1, std::placeholders::_2));
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(),
107 std::placeholders::_1, std::placeholders::_2));
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(),
113 std::placeholders::_2));
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(),
119 std::placeholders::_1, std::placeholders::_2));
121 handle_set_target_service = this->node_->create_service<canopen_interfaces::srv::COTargetDouble>(
122 std::string(this->node_->get_name()).append(
"/target").c_str(),
125 std::placeholders::_2));
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(),
138 std::placeholders::_1, std::placeholders::_2));
140 handle_enable_service = this->node_->create_service<std_srvs::srv::Trigger>(
141 std::string(this->node_->get_name()).append(
"/enable").c_str(),
144 std::placeholders::_1, std::placeholders::_2));
146 handle_disable_service = this->node_->create_service<std_srvs::srv::Trigger>(
147 std::string(this->node_->get_name()).append(
"/disable").c_str(),
150 std::placeholders::_1, std::placeholders::_2));
152 handle_halt_service = this->node_->create_service<std_srvs::srv::Trigger>(
153 std::string(this->node_->get_name()).append(
"/halt").c_str(),
156 std::placeholders::_1, std::placeholders::_2));
158 handle_recover_service = this->node_->create_service<std_srvs::srv::Trigger>(
159 std::string(this->node_->get_name()).append(
"/recover").c_str(),
162 std::placeholders::_1, std::placeholders::_2));
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(),
168 std::placeholders::_1, std::placeholders::_2));
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(),
174 std::placeholders::_1, std::placeholders::_2));
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(),
180 std::placeholders::_1, std::placeholders::_2));
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(),
186 std::placeholders::_1, std::placeholders::_2));
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(),
193 this, std::placeholders::_1, std::placeholders::_2));
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(),
199 std::placeholders::_1, std::placeholders::_2));
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(),
205 std::placeholders::_1, std::placeholders::_2));
207 handle_set_target_service = this->node_->create_service<canopen_interfaces::srv::COTargetDouble>(
208 std::string(this->node_->get_name()).append(
"/target").c_str(),
211 std::placeholders::_1, std::placeholders::_2));
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;
228 scale_pos_to_dev = std::optional(this->config_[
"scale_pos_to_dev"].as<double>());
235 scale_pos_from_dev = std::optional(this->config_[
"scale_pos_from_dev"].as<double>());
242 scale_vel_to_dev = std::optional(this->config_[
"scale_vel_to_dev"].as<double>());
249 scale_vel_from_dev = std::optional(this->config_[
"scale_vel_from_dev"].as<double>());
256 offset_pos_to_dev = std::optional(this->config_[
"offset_pos_to_dev"].as<double>());
263 offset_pos_from_dev = std::optional(this->config_[
"offset_from_to_dev"].as<double>());
270 switching_state = std::optional(this->config_[
"switching_state"].as<int>());
277 homing_timeout_seconds = std::optional(this->config_[
"homing_timout_seconds"].as<int>());
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);
293 homing_timeout_seconds_ = homing_timeout_seconds.value_or(10);
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_);
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;
317 scale_pos_to_dev = std::optional(this->config_[
"scale_pos_to_dev"].as<double>());
324 scale_pos_from_dev = std::optional(this->config_[
"scale_pos_from_dev"].as<double>());
331 scale_vel_to_dev = std::optional(this->config_[
"scale_vel_to_dev"].as<double>());
338 scale_vel_from_dev = std::optional(this->config_[
"scale_vel_from_dev"].as<double>());
345 offset_pos_to_dev = std::optional(this->config_[
"offset_pos_to_dev"].as<double>());
352 offset_pos_from_dev = std::optional(this->config_[
"offset_from_to_dev"].as<double>());
359 switching_state = std::optional(this->config_[
"switching_state"].as<int>());
366 homing_timeout_seconds = std::optional(this->config_[
"homing_timeout_seconds"].as<int>());
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);
382 homing_timeout_seconds_ = homing_timeout_seconds.value_or(10);
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_);
392template <
class NODETYPE>
396 motor_->registerDefaultModes();
397 motor_->set_diagnostic_status_msgs(this->diagnostic_collector_, this->diagnostic_enabled_);
400template <
class NODETYPE>
407template <
class NODETYPE>
411 motor_->handleRead();
412 motor_->handleWrite();
416template <
class NODETYPE>
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);
427template <
class NODETYPE>
432 std::make_shared<Motor402>(this->lely_driver_, switching_state_, homing_timeout_seconds_);
435template <
class NODETYPE>
437 const std_srvs::srv::Trigger::Request::SharedPtr request,
438 std_srvs::srv::Trigger::Response::SharedPtr response)
440 if (this->activated_.load())
442 bool temp = motor_->handleInit();
443 response->success = temp;
446template <
class NODETYPE>
448 const std_srvs::srv::Trigger::Request::SharedPtr request,
449 std_srvs::srv::Trigger::Response::SharedPtr response)
451 if (this->activated_.load())
453 response->success = motor_->handleRecover();
456template <
class NODETYPE>
458 const std_srvs::srv::Trigger::Request::SharedPtr request,
459 std_srvs::srv::Trigger::Response::SharedPtr response)
461 if (this->activated_.load())
463 response->success = motor_->handleHalt();
466template <
class NODETYPE>
468 const std_srvs::srv::Trigger::Request::SharedPtr request,
469 std_srvs::srv::Trigger::Response::SharedPtr response)
474template <
class NODETYPE>
476 const std_srvs::srv::Trigger::Request::SharedPtr request,
477 std_srvs::srv::Trigger::Response::SharedPtr response)
482template <
class NODETYPE>
484 const std_srvs::srv::Trigger::Request::SharedPtr request,
485 std_srvs::srv::Trigger::Response::SharedPtr response)
490template <
class NODETYPE>
492 const std_srvs::srv::Trigger::Request::SharedPtr request,
493 std_srvs::srv::Trigger::Response::SharedPtr response)
498template <
class NODETYPE>
500 const std_srvs::srv::Trigger::Request::SharedPtr request,
501 std_srvs::srv::Trigger::Response::SharedPtr response)
505template <
class NODETYPE>
507 const std_srvs::srv::Trigger::Request::SharedPtr request,
508 std_srvs::srv::Trigger::Response::SharedPtr response)
513template <
class NODETYPE>
515 const std_srvs::srv::Trigger::Request::SharedPtr request,
516 std_srvs::srv::Trigger::Response::SharedPtr response)
521template <
class NODETYPE>
523 const canopen_interfaces::srv::COTargetDouble::Request::SharedPtr request,
524 canopen_interfaces::srv::COTargetDouble::Response::SharedPtr response)
526 if (this->activated_.load())
528 auto mode = motor_->getMode();
534 target = request->target * scale_pos_to_dev_ + offset_pos_to_dev_;
540 target = request->target * scale_vel_to_dev_;
544 target = request->target;
547 response->success = motor_->setTarget(target);
551template <
class NODETYPE>
553 const std_srvs::srv::Trigger::Request::SharedPtr request,
554 std_srvs::srv::Trigger::Response::SharedPtr response)
556 if (this->activated_.load())
558 bool temp = motor_->handleDisable();
559 response->success = temp;
563template <
class NODETYPE>
565 const std_srvs::srv::Trigger::Request::SharedPtr request,
566 std_srvs::srv::Trigger::Response::SharedPtr response)
568 if (this->activated_.load())
570 bool temp = motor_->handleEnable();
571 response->success = temp;
575template <
class NODETYPE>
578 if (this->activated_.load())
580 bool temp = motor_->handleInit();
585 RCLCPP_INFO(this->node_->get_logger(),
"Initialisation failed.");
590template <
class NODETYPE>
593 if (this->activated_.load())
595 return motor_->handleRecover();
603template <
class NODETYPE>
606 if (this->activated_.load())
608 return motor_->handleHalt();
616template <
class NODETYPE>
619 if (this->activated_.load())
621 if (motor_->getMode() != mode)
623 return motor_->enterModeAndWait(mode);
633template <
class NODETYPE>
636 if (this->activated_.load())
638 auto mode = motor_->getMode();
639 double scaled_target;
644 scaled_target = target * scale_pos_to_dev_ + offset_pos_to_dev_;
650 scaled_target = target * scale_vel_to_dev_;
654 scaled_target = target;
657 return motor_->setTarget(scaled_target);
665template <
class NODETYPE>
667 diagnostic_updater::DiagnosticStatusWrapper & stat)
669 this->motor_->handleDiag();
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"));
@ 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