ros2_canopen
master
C++ ROS CANopen Library
|
#include <node_canopen_402_driver.hpp>
Public Member Functions | |
NodeCanopen402Driver (NODETYPE *node) | |
virtual void | init (bool called_from_base) override |
Initialises the driver. More... | |
virtual void | configure (bool called_from_base) override |
Configure the driver. More... | |
virtual void | activate (bool called_from_base) override |
Activates the driver. More... | |
virtual void | deactivate (bool called_from_base) override |
Deactivates the driver. More... | |
virtual void | add_to_master () override |
Add the driver to master. More... | |
virtual double | get_speed () |
virtual double | get_position () |
virtual uint16_t | get_mode () |
void | handle_init (const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response) |
Service Callback to initialise device. More... | |
bool | init_motor () |
Method to initialise device. More... | |
void | handle_recover (const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response) |
Service Callback to recover device. More... | |
bool | recover_motor () |
Method to recover device. More... | |
void | handle_halt (const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response) |
Service Callback to halt device. More... | |
bool | halt_motor () |
Method to halt device. More... | |
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. More... | |
bool | set_operation_mode (uint16_t mode) |
bool | set_mode_position () |
Method to set profiled position mode. More... | |
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. More... | |
bool | set_mode_velocity () |
Method to set profiled velocity mode. More... | |
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. More... | |
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. More... | |
bool | set_mode_interpolated_position () |
Method to set interpolated position mode. More... | |
bool | set_mode_cyclic_position () |
Method to set cyclic position mode. More... | |
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. More... | |
bool | set_mode_cyclic_velocity () |
Method to set cyclic velocity mode. More... | |
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. More... | |
bool | set_mode_torque () |
Method to set profiled torque mode. More... | |
void | handle_set_target (const canopen_interfaces::srv::COTargetDouble::Request::SharedPtr request, canopen_interfaces::srv::COTargetDouble::Response::SharedPtr response) |
Service Callback to set target. More... | |
bool | set_target (double target) |
Method to set target. More... | |
![]() | |
NodeCanopenProxyDriver (NODETYPE *node) | |
virtual bool | reset_node_nmt_command () |
virtual bool | start_node_nmt_command () |
virtual bool | tpdo_transmit (COData &data) |
virtual bool | sdo_write (COData &data) |
virtual bool | sdo_read (COData &data) |
![]() | |
NodeCanopenBaseDriver (NODETYPE *node) | |
virtual | ~NodeCanopenBaseDriver () |
virtual void | cleanup (bool called_from_base) |
Cleanup the driver. More... | |
virtual void | shutdown (bool called_from_base) |
Shuts down the driver. More... | |
virtual void | remove_from_master () |
Remove the driver from master. More... | |
void | register_nmt_state_cb (std::function< void(canopen::NmtState, uint8_t)> nmt_state_cb) |
Register a callback for NMT state change. More... | |
void | register_rpdo_cb (std::function< void(COData, uint8_t)> rpdo_cb) |
Register a callback for RPDO. More... | |
void | register_emcy_cb (std::function< void(COEmcy, uint8_t)> emcy_cb) |
Register a callback for EMCY. More... | |
![]() | |
NodeCanopenDriver (NODETYPE *node) | |
virtual void | set_master (std::shared_ptr< lely::ev::Executor > exec, std::shared_ptr< lely::canopen::AsyncMaster > master) |
Set Master. More... | |
void | init () |
Initialise the driver. More... | |
void | configure () |
Configure the driver. More... | |
void | activate () |
Activate the driver. More... | |
void | deactivate () |
Deactivate the driver. More... | |
void | cleanup () |
Cleanup the driver. More... | |
void | shutdown () |
Shutdown the driver. More... | |
virtual void | demand_set_master () |
Demand set Master. More... | |
![]() | |
NodeCanopenDriverInterface () | |
Protected Member Functions | |
void | publish () |
virtual void | poll_timer_callback () override |
void | diagnostic_callback (diagnostic_updater::DiagnosticStatusWrapper &stat) override |
![]() | |
virtual void | on_nmt (canopen::NmtState nmt_state) override |
virtual void | on_rpdo (COData data) override |
virtual void | on_tpdo (const canopen_interfaces::msg::COData::SharedPtr msg) |
void | on_nmt_state_reset (const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response) |
void | on_nmt_state_start (const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response) |
void | on_sdo_read (const canopen_interfaces::srv::CORead::Request::SharedPtr request, canopen_interfaces::srv::CORead::Response::SharedPtr response) |
void | on_sdo_write (const canopen_interfaces::srv::COWrite::Request::SharedPtr request, canopen_interfaces::srv::COWrite::Response::SharedPtr response) |
![]() | |
void | nmt_listener () |
void | rdpo_listener () |
void | emcy_listener () |
virtual void | on_emcy (COEmcy emcy) |
Protected Attributes | |
std::shared_ptr< Motor402 > | motor_ |
rclcpp::TimerBase::SharedPtr | timer_ |
rclcpp::Service< std_srvs::srv::Trigger >::SharedPtr | handle_init_service |
rclcpp::Service< std_srvs::srv::Trigger >::SharedPtr | handle_halt_service |
rclcpp::Service< std_srvs::srv::Trigger >::SharedPtr | handle_recover_service |
rclcpp::Service< std_srvs::srv::Trigger >::SharedPtr | handle_set_mode_position_service |
rclcpp::Service< std_srvs::srv::Trigger >::SharedPtr | handle_set_mode_torque_service |
rclcpp::Service< std_srvs::srv::Trigger >::SharedPtr | handle_set_mode_velocity_service |
rclcpp::Service< std_srvs::srv::Trigger >::SharedPtr | handle_set_mode_cyclic_velocity_service |
rclcpp::Service< std_srvs::srv::Trigger >::SharedPtr | handle_set_mode_cyclic_position_service |
rclcpp::Service< std_srvs::srv::Trigger >::SharedPtr | handle_set_mode_interpolated_position_service |
rclcpp::Service< canopen_interfaces::srv::COTargetDouble >::SharedPtr | handle_set_target_service |
rclcpp::Publisher< sensor_msgs::msg::JointState >::SharedPtr | publish_joint_state |
double | scale_pos_to_dev_ |
double | scale_pos_from_dev_ |
double | scale_vel_to_dev_ |
double | scale_vel_from_dev_ |
ros2_canopen::State402::InternalState | switching_state_ |
![]() | |
rclcpp::Publisher< std_msgs::msg::String >::SharedPtr | nmt_state_publisher |
rclcpp::Publisher< canopen_interfaces::msg::COData >::SharedPtr | rpdo_publisher |
rclcpp::Subscription< canopen_interfaces::msg::COData >::SharedPtr | tpdo_subscriber |
rclcpp::Service< std_srvs::srv::Trigger >::SharedPtr | nmt_state_reset_service |
rclcpp::Service< std_srvs::srv::Trigger >::SharedPtr | nmt_state_start_service |
rclcpp::Service< canopen_interfaces::srv::CORead >::SharedPtr | sdo_read_service |
rclcpp::Service< canopen_interfaces::srv::COWrite >::SharedPtr | sdo_write_service |
std::mutex | sdo_mtex |
![]() | |
std::thread | nmt_state_publisher_thread_ |
std::thread | rpdo_publisher_thread_ |
std::thread | emcy_publisher_thread_ |
std::mutex | driver_mutex_ |
std::shared_ptr< ros2_canopen::LelyDriverBridge > | lely_driver_ |
uint32_t | period_ms_ |
int | sdo_timeout_ms_ |
bool | polling_ |
std::function< void(canopen::NmtState, uint8_t)> | nmt_state_cb_ |
std::function< void(COData, uint8_t)> | rpdo_cb_ |
std::function< void(COEmcy, uint8_t)> | emcy_cb_ |
std::shared_ptr< ros2_canopen::SafeQueue< ros2_canopen::COEmcy > > | emcy_queue_ |
std::shared_ptr< ros2_canopen::SafeQueue< ros2_canopen::COData > > | rpdo_queue_ |
rclcpp::TimerBase::SharedPtr | poll_timer_ |
std::atomic< bool > | diagnostic_enabled_ |
uint32_t | diagnostic_period_ms_ |
std::shared_ptr< diagnostic_updater::Updater > | diagnostic_updater_ |
std::shared_ptr< DiagnosticsCollector > | diagnostic_collector_ |
![]() | |
NODETYPE * | node_ |
std::shared_ptr< lely::ev::Executor > | exec_ |
std::shared_ptr< lely::canopen::AsyncMaster > | master_ |
std::shared_ptr< lely::canopen::BasicDriver > | driver_ |
std::chrono::milliseconds | non_transmit_timeout_ |
YAML::Node | config_ |
uint8_t | node_id_ |
std::string | container_name_ |
std::string | eds_ |
std::string | bin_ |
rclcpp::CallbackGroup::SharedPtr | client_cbg_ |
rclcpp::CallbackGroup::SharedPtr | timer_cbg_ |
std::atomic< bool > | master_set_ |
std::atomic< bool > | initialised_ |
std::atomic< bool > | configured_ |
std::atomic< bool > | activated_ |
NodeCanopen402Driver::NodeCanopen402Driver | ( | NODETYPE * | node | ) |
|
protected |
|
overrideprotectedvirtual |
Reimplemented from ros2_canopen::node_interfaces::NodeCanopenBaseDriver< NODETYPE >.
|
overrideprotectedvirtual |
Reimplemented from ros2_canopen::node_interfaces::NodeCanopenProxyDriver< NODETYPE >.
|
overridevirtual |
Initialises the driver.
This does not do anything, it is an empty function. it should be overridden by derived classes.
called_from_base |
Reimplemented from ros2_canopen::node_interfaces::NodeCanopenProxyDriver< NODETYPE >.
|
overridevirtual |
Configure the driver.
This function should be overridden by derived classes.
called_from_base |
Reimplemented from ros2_canopen::node_interfaces::NodeCanopenBaseDriver< NODETYPE >.
|
overridevirtual |
Activates the driver.
This function should be overridden by derived classes.
called_from_base |
Reimplemented from ros2_canopen::node_interfaces::NodeCanopenBaseDriver< NODETYPE >.
|
overridevirtual |
Deactivates the driver.
This function should be overridden by derived classes.
called_from_base |
Reimplemented from ros2_canopen::node_interfaces::NodeCanopenBaseDriver< NODETYPE >.
|
overridevirtual |
Add the driver to master.
Reimplemented from ros2_canopen::node_interfaces::NodeCanopenBaseDriver< NODETYPE >.
|
inlinevirtual |
|
inlinevirtual |
|
inlinevirtual |
void NodeCanopen402Driver::handle_init | ( | const std_srvs::srv::Trigger::Request::SharedPtr | request, |
std_srvs::srv::Trigger::Response::SharedPtr | response | ||
) |
Service Callback to initialise device.
Calls Motor402::handleInit function. Brings motor to enabled state and homes it.
[in] | request | |
[out] | response |
bool NodeCanopen402Driver::init_motor |
Method to initialise device.
Calls Motor402::handleInit function. Brings motor to enabled state and homes it.
[in] | void |
void NodeCanopen402Driver::handle_recover | ( | const std_srvs::srv::Trigger::Request::SharedPtr | request, |
std_srvs::srv::Trigger::Response::SharedPtr | response | ||
) |
Service Callback to recover device.
Calls Motor402::handleRecover function. Resets faults and brings motor to enabled state.
[in] | request | |
[out] | response |
bool NodeCanopen402Driver::recover_motor |
Method to recover device.
Calls Motor402::handleRecover function. Resets faults and brings motor to enabled state.
[in] | void |
void NodeCanopen402Driver::handle_halt | ( | const std_srvs::srv::Trigger::Request::SharedPtr | request, |
std_srvs::srv::Trigger::Response::SharedPtr | response | ||
) |
Service Callback to halt device.
Calls Motor402::handleHalt function. Calls Quickstop. Resulting Motor state depends on devices configuration specifically object 0x605A.
[in] | request | |
[out] | response |
bool NodeCanopen402Driver::halt_motor |
Method to halt device.
Calls Motor402::handleHalt function. Calls Quickstop. Resulting Motor state depends on devices configuration specifically object 0x605A.
[in] | void |
void NodeCanopen402Driver::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.
Calls Motor402::enterModeAndWait with Profiled Position Mode as Target Operation Mode. If successful, the motor was transitioned to Profiled Position Mode.
[in] | request | |
[out] | response |
bool NodeCanopen402Driver::set_operation_mode | ( | uint16_t | mode | ) |
bool NodeCanopen402Driver::set_mode_position |
Method to set profiled position mode.
Calls Motor402::enterModeAndWait with Profiled Position Mode as Target Operation Mode. If successful, the motor was transitioned to Profiled Position Mode.
[in] | void |
void NodeCanopen402Driver::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.
Calls Motor402::enterModeAndWait with Profiled Velocity Mode as Target Operation Mode. If successful, the motor was transitioned to Profiled Velocity Mode.
[in] | request | |
[out] | response |
bool NodeCanopen402Driver::set_mode_velocity |
Method to set profiled velocity mode.
Calls Motor402::enterModeAndWait with Profiled Velocity Mode as Target Operation Mode. If successful, the motor was transitioned to Profiled Velocity Mode.
[in] | void |
void NodeCanopen402Driver::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.
Calls Motor402::enterModeAndWait with Cyclic Position Mode as Target Operation Mode. If successful, the motor was transitioned to Cyclic Position Mode.
[in] | request | |
[out] | response |
void NodeCanopen402Driver::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.
Calls Motor402::enterModeAndWait with Interpolated Position Mode as Target Operation Mode. If successful, the motor was transitioned to Interpolated Position Mode. This only supports linear mode.
[in] | request | |
[out] | response |
bool NodeCanopen402Driver::set_mode_interpolated_position |
Method to set interpolated position mode.
Calls Motor402::enterModeAndWait with Interpolated Position Mode as Target Operation Mode. If successful, the motor was transitioned to Interpolated Position Mode. This only supports linear mode.
[in] | void | |
[out] | bool |
bool NodeCanopen402Driver::set_mode_cyclic_position |
Method to set cyclic position mode.
Calls Motor402::enterModeAndWait with Cyclic Position Mode as Target Operation Mode. If successful, the motor was transitioned to Cyclic Position Mode.
[in] | void |
void NodeCanopen402Driver::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.
Calls Motor402::enterModeAndWait with Cyclic Velocity Mode as Target Operation Mode. If successful, the motor was transitioned to Cyclic Velocity Mode.
[in] | request | |
[out] | response |
bool NodeCanopen402Driver::set_mode_cyclic_velocity |
Method to set cyclic velocity mode.
Calls Motor402::enterModeAndWait with Cyclic Velocity Mode as Target Operation Mode. If successful, the motor was transitioned to Cyclic Velocity Mode.
[in] | void |
void NodeCanopen402Driver::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.
Calls Motor402::enterModeAndWait with Profiled Torque Mode as Target Operation Mode. If successful, the motor was transitioned to Profiled Torque Mode.
[in] | request | |
[out] | response |
bool NodeCanopen402Driver::set_mode_torque |
Method to set profiled torque mode.
Calls Motor402::enterModeAndWait with Profiled Torque Mode as Target Operation Mode. If successful, the motor was transitioned to Profiled Torque Mode.
[in] | void |
void NodeCanopen402Driver::handle_set_target | ( | const canopen_interfaces::srv::COTargetDouble::Request::SharedPtr | request, |
canopen_interfaces::srv::COTargetDouble::Response::SharedPtr | response | ||
) |
Service Callback to set target.
Calls Motor402::setTarget and sets the requested target value. Note that the resulting movement is dependent on the Operation Mode and the drives state.
[in] | request | |
[out] | response |
bool NodeCanopen402Driver::set_target | ( | double | target | ) |
Method to set target.
Calls Motor402::setTarget and sets the requested target value. Note that the resulting movement is dependent on the Operation Mode and the drives state.
[in] | double | target value |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |