ros2_canopen  master
C++ ROS CANopen Library
node_canopen_402_driver.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
20 #define NODE_CANOPEN_402_DRIVER
21 
24 #include "canopen_interfaces/srv/co_target_double.hpp"
26 #include "sensor_msgs/msg/joint_state.hpp"
27 
28 namespace ros2_canopen
29 {
30 namespace node_interfaces
31 {
32 
33 template <class NODETYPE>
35 {
36  static_assert(
37  std::is_base_of<rclcpp::Node, NODETYPE>::value ||
38  std::is_base_of<rclcpp_lifecycle::LifecycleNode, NODETYPE>::value,
39  "NODETYPE must derive from rclcpp::Node or rclcpp_lifecycle::LifecycleNode");
40 
41 protected:
42  std::shared_ptr<Motor402> motor_;
43  rclcpp::TimerBase::SharedPtr timer_;
44  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_init_service;
45  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_halt_service;
46  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_recover_service;
47  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_set_mode_position_service;
48  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_set_mode_torque_service;
49  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_set_mode_velocity_service;
50  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_set_mode_cyclic_velocity_service;
51  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_set_mode_cyclic_position_service;
52  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_set_mode_interpolated_position_service;
53  rclcpp::Service<canopen_interfaces::srv::COTargetDouble>::SharedPtr handle_set_target_service;
54  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr publish_joint_state;
60 
61  void publish();
62  virtual void poll_timer_callback() override;
63  void diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat) override;
64 
65 public:
66  NodeCanopen402Driver(NODETYPE * node);
67 
68  virtual void init(bool called_from_base) override;
69  virtual void configure(bool called_from_base) override;
70  virtual void activate(bool called_from_base) override;
71  virtual void deactivate(bool called_from_base) override;
72  virtual void add_to_master() override;
73 
74  virtual double get_speed() { return motor_->get_speed() * scale_vel_from_dev_; }
75 
76  virtual double get_position() { return motor_->get_position() * scale_pos_from_dev_; }
77 
78  virtual uint16_t get_mode() { return motor_->getMode(); }
79 
89  void handle_init(
90  const std_srvs::srv::Trigger::Request::SharedPtr request,
91  std_srvs::srv::Trigger::Response::SharedPtr response);
92 
104  bool init_motor();
105 
115  void handle_recover(
116  const std_srvs::srv::Trigger::Request::SharedPtr request,
117  std_srvs::srv::Trigger::Response::SharedPtr response);
118 
129  bool recover_motor();
130 
141  void handle_halt(
142  const std_srvs::srv::Trigger::Request::SharedPtr request,
143  std_srvs::srv::Trigger::Response::SharedPtr response);
144 
156  bool halt_motor();
157 
169  const std_srvs::srv::Trigger::Request::SharedPtr request,
170  std_srvs::srv::Trigger::Response::SharedPtr response);
171 
172  bool set_operation_mode(uint16_t mode);
173 
185  bool set_mode_position();
186 
198  const std_srvs::srv::Trigger::Request::SharedPtr request,
199  std_srvs::srv::Trigger::Response::SharedPtr response);
200 
212  bool set_mode_velocity();
213 
225  const std_srvs::srv::Trigger::Request::SharedPtr request,
226  std_srvs::srv::Trigger::Response::SharedPtr response);
227 
239  const std_srvs::srv::Trigger::Request::SharedPtr request,
240  std_srvs::srv::Trigger::Response::SharedPtr response);
241 
253 
266 
278  const std_srvs::srv::Trigger::Request::SharedPtr request,
279  std_srvs::srv::Trigger::Response::SharedPtr response);
280 
293 
305  const std_srvs::srv::Trigger::Request::SharedPtr request,
306  std_srvs::srv::Trigger::Response::SharedPtr response);
307 
319  bool set_mode_torque();
320 
331  void handle_set_target(
332  const canopen_interfaces::srv::COTargetDouble::Request::SharedPtr request,
333  canopen_interfaces::srv::COTargetDouble::Response::SharedPtr response);
334 
346  bool set_target(double target);
347 };
348 } // namespace node_interfaces
349 } // namespace ros2_canopen
350 
351 #endif
InternalState
Definition: state.hpp:50
Definition: node_canopen_402_driver.hpp:35
double scale_vel_from_dev_
Definition: node_canopen_402_driver.hpp:58
double scale_vel_to_dev_
Definition: node_canopen_402_driver.hpp:57
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:402
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:360
rclcpp::Service< std_srvs::srv::Trigger >::SharedPtr handle_set_mode_interpolated_position_service
Definition: node_canopen_402_driver.hpp:52
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:339
rclcpp::Service< std_srvs::srv::Trigger >::SharedPtr handle_set_mode_torque_service
Definition: node_canopen_402_driver.hpp:48
bool set_target(double target)
Method to set target.
Definition: node_canopen_402_driver_impl.hpp:618
rclcpp::Service< std_srvs::srv::Trigger >::SharedPtr handle_halt_service
Definition: node_canopen_402_driver.hpp:45
bool set_mode_velocity()
Method to set profiled velocity mode.
Definition: node_canopen_402_driver_impl.hpp:538
virtual double get_position()
Definition: node_canopen_402_driver.hpp:76
rclcpp::Service< std_srvs::srv::Trigger >::SharedPtr handle_set_mode_cyclic_position_service
Definition: node_canopen_402_driver.hpp:51
double scale_pos_to_dev_
Definition: node_canopen_402_driver.hpp:55
bool set_mode_torque()
Method to set profiled torque mode.
Definition: node_canopen_402_driver_impl.hpp:598
rclcpp::Service< std_srvs::srv::Trigger >::SharedPtr handle_set_mode_position_service
Definition: node_canopen_402_driver.hpp:47
rclcpp::TimerBase::SharedPtr timer_
Definition: node_canopen_402_driver.hpp:43
virtual void configure(bool called_from_base) override
Configure the driver.
rclcpp::Service< canopen_interfaces::srv::COTargetDouble >::SharedPtr handle_set_target_service
Definition: node_canopen_402_driver.hpp:53
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:417
rclcpp::Service< std_srvs::srv::Trigger >::SharedPtr handle_set_mode_velocity_service
Definition: node_canopen_402_driver.hpp:49
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:378
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:394
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:409
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:386
virtual void poll_timer_callback() override
Definition: node_canopen_402_driver_impl.hpp:312
rclcpp::Service< std_srvs::srv::Trigger >::SharedPtr handle_init_service
Definition: node_canopen_402_driver.hpp:44
bool set_mode_interpolated_position()
Method to set interpolated position mode.
Definition: node_canopen_402_driver_impl.hpp:518
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:370
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:350
void diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper &stat) override
Definition: node_canopen_402_driver_impl.hpp:650
bool halt_motor()
Method to halt device.
Definition: node_canopen_402_driver_impl.hpp:475
bool set_mode_position()
Method to set profiled position mode.
Definition: node_canopen_402_driver_impl.hpp:498
std::shared_ptr< Motor402 > motor_
Definition: node_canopen_402_driver.hpp:39
bool init_motor()
Method to initialise device.
Definition: node_canopen_402_driver_impl.hpp:447
bool recover_motor()
Method to recover device.
Definition: node_canopen_402_driver_impl.hpp:462
ros2_canopen::State402::InternalState switching_state_
Definition: node_canopen_402_driver.hpp:59
virtual uint16_t get_mode()
Definition: node_canopen_402_driver.hpp:78
rclcpp::Service< std_srvs::srv::Trigger >::SharedPtr handle_recover_service
Definition: node_canopen_402_driver.hpp:46
rclcpp::Service< std_srvs::srv::Trigger >::SharedPtr handle_set_mode_cyclic_velocity_service
Definition: node_canopen_402_driver.hpp:50
NodeCanopen402Driver(NODETYPE *node)
Definition: node_canopen_402_driver_impl.hpp:31
double scale_pos_from_dev_
Definition: node_canopen_402_driver.hpp:56
void publish()
Definition: node_canopen_402_driver_impl.hpp:321
bool set_operation_mode(uint16_t mode)
Definition: node_canopen_402_driver_impl.hpp:488
rclcpp::Publisher< sensor_msgs::msg::JointState >::SharedPtr publish_joint_state
Definition: node_canopen_402_driver.hpp:54
bool set_mode_cyclic_velocity()
Method to set cyclic velocity mode.
Definition: node_canopen_402_driver_impl.hpp:578
virtual double get_speed()
Definition: node_canopen_402_driver.hpp:74
bool set_mode_cyclic_position()
Method to set cyclic position mode.
Definition: node_canopen_402_driver_impl.hpp:558
virtual void add_to_master() override
Add the driver to master.
Definition: node_canopen_402_driver_impl.hpp:332
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
Definition: node_canopen_proxy_driver.hpp:26
Definition: configuration_manager.hpp:28