19 #ifndef NODE_CANOPEN_402_DRIVER
20 #define NODE_CANOPEN_402_DRIVER
24 #include "canopen_interfaces/srv/co_target_double.hpp"
26 #include "sensor_msgs/msg/joint_state.hpp"
30 namespace node_interfaces
33 template <
class NODETYPE>
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");
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;
90 const std_srvs::srv::Trigger::Request::SharedPtr request,
91 std_srvs::srv::Trigger::Response::SharedPtr response);
116 const std_srvs::srv::Trigger::Request::SharedPtr request,
117 std_srvs::srv::Trigger::Response::SharedPtr response);
142 const std_srvs::srv::Trigger::Request::SharedPtr request,
143 std_srvs::srv::Trigger::Response::SharedPtr response);
169 const std_srvs::srv::Trigger::Request::SharedPtr request,
170 std_srvs::srv::Trigger::Response::SharedPtr response);
198 const std_srvs::srv::Trigger::Request::SharedPtr request,
199 std_srvs::srv::Trigger::Response::SharedPtr response);
225 const std_srvs::srv::Trigger::Request::SharedPtr request,
226 std_srvs::srv::Trigger::Response::SharedPtr response);
239 const std_srvs::srv::Trigger::Request::SharedPtr request,
240 std_srvs::srv::Trigger::Response::SharedPtr response);
278 const std_srvs::srv::Trigger::Request::SharedPtr request,
279 std_srvs::srv::Trigger::Response::SharedPtr response);
305 const std_srvs::srv::Trigger::Request::SharedPtr request,
306 std_srvs::srv::Trigger::Response::SharedPtr response);
332 const canopen_interfaces::srv::COTargetDouble::Request::SharedPtr request,
333 canopen_interfaces::srv::COTargetDouble::Response::SharedPtr response);
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