Program Listing for File node_canopen_402_driver_impl.hpp
↰ Return to documentation for file (/tmp/ws/src/ros2_canopen/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp
)
// Copyright 2023 Christoph Hellmann Santos
// Vishnuprasad Prachandabhanu
// Lovro Ivanov
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <https://www.gnu.org/licenses/>.
//
#ifndef NODE_CANOPEN_402_DRIVER_IMPL_HPP_
#define NODE_CANOPEN_402_DRIVER_IMPL_HPP_
#include "canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp"
#include "canopen_core/driver_error.hpp"
#include <optional>
using namespace ros2_canopen::node_interfaces;
using namespace std::placeholders;
template <class NODETYPE>
NodeCanopen402Driver<NODETYPE>::NodeCanopen402Driver(NODETYPE * node)
: ros2_canopen::node_interfaces::NodeCanopenProxyDriver<NODETYPE>(node)
{
}
template <class NODETYPE>
void NodeCanopen402Driver<NODETYPE>::init(bool called_from_base)
{
RCLCPP_ERROR(this->node_->get_logger(), "Not init implemented.");
}
template <>
void NodeCanopen402Driver<rclcpp::Node>::init(bool called_from_base)
{
NodeCanopenProxyDriver<rclcpp::Node>::init(false);
publish_joint_state =
this->node_->create_publisher<sensor_msgs::msg::JointState>("~/joint_states", 1);
handle_init_service = this->node_->create_service<std_srvs::srv::Trigger>(
std::string(this->node_->get_name()).append("/init").c_str(),
std::bind(
&NodeCanopen402Driver<rclcpp::Node>::handle_init, this, std::placeholders::_1,
std::placeholders::_2));
handle_halt_service = this->node_->create_service<std_srvs::srv::Trigger>(
std::string(this->node_->get_name()).append("/halt").c_str(),
std::bind(
&NodeCanopen402Driver<rclcpp::Node>::handle_halt, this, std::placeholders::_1,
std::placeholders::_2));
handle_recover_service = this->node_->create_service<std_srvs::srv::Trigger>(
std::string(this->node_->get_name()).append("/recover").c_str(),
std::bind(
&NodeCanopen402Driver<rclcpp::Node>::handle_recover, this, std::placeholders::_1,
std::placeholders::_2));
handle_set_mode_position_service = this->node_->create_service<std_srvs::srv::Trigger>(
std::string(this->node_->get_name()).append("/position_mode").c_str(),
std::bind(
&NodeCanopen402Driver<rclcpp::Node>::handle_set_mode_position, this, std::placeholders::_1,
std::placeholders::_2));
handle_set_mode_velocity_service = this->node_->create_service<std_srvs::srv::Trigger>(
std::string(this->node_->get_name()).append("/velocity_mode").c_str(),
std::bind(
&NodeCanopen402Driver<rclcpp::Node>::handle_set_mode_velocity, this, std::placeholders::_1,
std::placeholders::_2));
handle_set_mode_cyclic_velocity_service = this->node_->create_service<std_srvs::srv::Trigger>(
std::string(this->node_->get_name()).append("/cyclic_velocity_mode").c_str(),
std::bind(
&NodeCanopen402Driver<rclcpp::Node>::handle_set_mode_cyclic_velocity, this,
std::placeholders::_1, std::placeholders::_2));
handle_set_mode_cyclic_position_service = this->node_->create_service<std_srvs::srv::Trigger>(
std::string(this->node_->get_name()).append("/cyclic_position_mode").c_str(),
std::bind(
&NodeCanopen402Driver<rclcpp::Node>::handle_set_mode_cyclic_position, this,
std::placeholders::_1, std::placeholders::_2));
handle_set_mode_interpolated_position_service =
this->node_->create_service<std_srvs::srv::Trigger>(
std::string(this->node_->get_name()).append("/interpolated_position_mode").c_str(),
std::bind(
&NodeCanopen402Driver<rclcpp::Node>::handle_set_mode_interpolated_position, this,
std::placeholders::_1, std::placeholders::_2));
handle_set_mode_torque_service = this->node_->create_service<std_srvs::srv::Trigger>(
std::string(this->node_->get_name()).append("/torque_mode").c_str(),
std::bind(
&NodeCanopen402Driver<rclcpp::Node>::handle_set_mode_torque, this, std::placeholders::_1,
std::placeholders::_2));
handle_set_target_service = this->node_->create_service<canopen_interfaces::srv::COTargetDouble>(
std::string(this->node_->get_name()).append("/target").c_str(),
std::bind(
&NodeCanopen402Driver<rclcpp::Node>::handle_set_target, this, std::placeholders::_1,
std::placeholders::_2));
}
template <>
void NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::init(bool called_from_base)
{
NodeCanopenProxyDriver<rclcpp_lifecycle::LifecycleNode>::init(false);
publish_joint_state =
this->node_->create_publisher<sensor_msgs::msg::JointState>("~/joint_states", 10);
handle_init_service = this->node_->create_service<std_srvs::srv::Trigger>(
std::string(this->node_->get_name()).append("/init").c_str(),
std::bind(
&NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::handle_init, this,
std::placeholders::_1, std::placeholders::_2));
handle_halt_service = this->node_->create_service<std_srvs::srv::Trigger>(
std::string(this->node_->get_name()).append("/halt").c_str(),
std::bind(
&NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::handle_halt, this,
std::placeholders::_1, std::placeholders::_2));
handle_recover_service = this->node_->create_service<std_srvs::srv::Trigger>(
std::string(this->node_->get_name()).append("/recover").c_str(),
std::bind(
&NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::handle_recover, this,
std::placeholders::_1, std::placeholders::_2));
handle_set_mode_position_service = this->node_->create_service<std_srvs::srv::Trigger>(
std::string(this->node_->get_name()).append("/position_mode").c_str(),
std::bind(
&NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::handle_set_mode_position, this,
std::placeholders::_1, std::placeholders::_2));
handle_set_mode_velocity_service = this->node_->create_service<std_srvs::srv::Trigger>(
std::string(this->node_->get_name()).append("/velocity_mode").c_str(),
std::bind(
&NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::handle_set_mode_velocity, this,
std::placeholders::_1, std::placeholders::_2));
handle_set_mode_cyclic_velocity_service = this->node_->create_service<std_srvs::srv::Trigger>(
std::string(this->node_->get_name()).append("/cyclic_velocity_mode").c_str(),
std::bind(
&NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::handle_set_mode_cyclic_velocity, this,
std::placeholders::_1, std::placeholders::_2));
handle_set_mode_cyclic_position_service = this->node_->create_service<std_srvs::srv::Trigger>(
std::string(this->node_->get_name()).append("/cyclic_position_mode").c_str(),
std::bind(
&NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::handle_set_mode_cyclic_position, this,
std::placeholders::_1, std::placeholders::_2));
handle_set_mode_interpolated_position_service = this->node_->create_service<
std_srvs::srv::Trigger>(
std::string(this->node_->get_name()).append("/interpolated_position_mode").c_str(),
std::bind(
&NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::handle_set_mode_interpolated_position,
this, std::placeholders::_1, std::placeholders::_2));
handle_set_mode_torque_service = this->node_->create_service<std_srvs::srv::Trigger>(
std::string(this->node_->get_name()).append("/torque_mode").c_str(),
std::bind(
&NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::handle_set_mode_torque, this,
std::placeholders::_1, std::placeholders::_2));
handle_set_target_service = this->node_->create_service<canopen_interfaces::srv::COTargetDouble>(
std::string(this->node_->get_name()).append("/target").c_str(),
std::bind(
&NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::handle_set_target, this,
std::placeholders::_1, std::placeholders::_2));
}
template <>
void NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::configure(bool called_from_base)
{
NodeCanopenProxyDriver<rclcpp_lifecycle::LifecycleNode>::configure(false);
std::optional<double> scale_pos_to_dev;
std::optional<double> scale_pos_from_dev;
std::optional<double> scale_vel_to_dev;
std::optional<double> scale_vel_from_dev;
std::optional<int> switching_state;
try
{
scale_pos_to_dev = std::optional(this->config_["scale_pos_to_dev"].as<double>());
}
catch (...)
{
}
try
{
scale_pos_from_dev = std::optional(this->config_["scale_pos_from_dev"].as<double>());
}
catch (...)
{
}
try
{
scale_vel_to_dev = std::optional(this->config_["scale_vel_to_dev"].as<double>());
}
catch (...)
{
}
try
{
scale_vel_from_dev = std::optional(this->config_["scale_vel_from_dev"].as<double>());
}
catch (...)
{
}
try
{
switching_state = std::optional(this->config_["switching_state"].as<int>());
}
catch (...)
{
}
// auto period = this->config_["scale_eff_to_dev"].as<double>();
// auto period = this->config_["scale_eff_from_dev"].as<double>();
scale_pos_to_dev_ = scale_pos_to_dev.value_or(1000.0);
scale_pos_from_dev_ = scale_pos_from_dev.value_or(0.001);
scale_vel_to_dev_ = scale_vel_to_dev.value_or(1000.0);
scale_vel_from_dev_ = scale_vel_from_dev.value_or(0.001);
switching_state_ = (ros2_canopen::State402::InternalState)switching_state.value_or(
(int)ros2_canopen::State402::InternalState::Operation_Enable);
RCLCPP_INFO(
this->node_->get_logger(),
"scale_pos_to_dev_ %f\nscale_pos_from_dev_ %f\nscale_vel_to_dev_ %f\nscale_vel_from_dev_ %f\n",
scale_pos_to_dev_, scale_pos_from_dev_, scale_vel_to_dev_, scale_vel_from_dev_);
}
template <>
void NodeCanopen402Driver<rclcpp::Node>::configure(bool called_from_base)
{
NodeCanopenProxyDriver<rclcpp::Node>::configure(false);
std::optional<double> scale_pos_to_dev;
std::optional<double> scale_pos_from_dev;
std::optional<double> scale_vel_to_dev;
std::optional<double> scale_vel_from_dev;
std::optional<int> switching_state;
try
{
scale_pos_to_dev = std::optional(this->config_["scale_pos_to_dev"].as<double>());
}
catch (...)
{
}
try
{
scale_pos_from_dev = std::optional(this->config_["scale_pos_from_dev"].as<double>());
}
catch (...)
{
}
try
{
scale_vel_to_dev = std::optional(this->config_["scale_vel_to_dev"].as<double>());
}
catch (...)
{
}
try
{
scale_vel_from_dev = std::optional(this->config_["scale_vel_from_dev"].as<double>());
}
catch (...)
{
}
try
{
switching_state = std::optional(this->config_["switching_state"].as<int>());
}
catch (...)
{
}
// auto period = this->config_["scale_eff_to_dev"].as<double>();
// auto period = this->config_["scale_eff_from_dev"].as<double>();
scale_pos_to_dev_ = scale_pos_to_dev.value_or(1000.0);
scale_pos_from_dev_ = scale_pos_from_dev.value_or(0.001);
scale_vel_to_dev_ = scale_vel_to_dev.value_or(1000.0);
scale_vel_from_dev_ = scale_vel_from_dev.value_or(0.001);
switching_state_ = (ros2_canopen::State402::InternalState)switching_state.value_or(
(int)ros2_canopen::State402::InternalState::Operation_Enable);
RCLCPP_INFO(
this->node_->get_logger(),
"scale_pos_to_dev_ %f\nscale_pos_from_dev_ %f\nscale_vel_to_dev_ %f\nscale_vel_from_dev_ %f\n",
scale_pos_to_dev_, scale_pos_from_dev_, scale_vel_to_dev_, scale_vel_from_dev_);
}
template <class NODETYPE>
void NodeCanopen402Driver<NODETYPE>::activate(bool called_from_base)
{
NodeCanopenProxyDriver<NODETYPE>::activate(false);
motor_->registerDefaultModes();
motor_->set_diagnostic_status_msgs(this->diagnostic_collector_, this->diagnostic_enabled_);
}
template <class NODETYPE>
void NodeCanopen402Driver<NODETYPE>::deactivate(bool called_from_base)
{
NodeCanopenProxyDriver<NODETYPE>::deactivate(false);
timer_->cancel();
}
template <class NODETYPE>
void NodeCanopen402Driver<NODETYPE>::poll_timer_callback()
{
NodeCanopenProxyDriver<NODETYPE>::poll_timer_callback();
motor_->handleRead();
motor_->handleWrite();
publish();
}
template <class NODETYPE>
void NodeCanopen402Driver<NODETYPE>::publish()
{
sensor_msgs::msg::JointState js_msg;
js_msg.name.push_back(this->node_->get_name());
js_msg.position.push_back(motor_->get_position() * scale_pos_from_dev_);
js_msg.velocity.push_back(motor_->get_speed() * scale_vel_from_dev_);
js_msg.effort.push_back(0.0);
publish_joint_state->publish(js_msg);
}
template <class NODETYPE>
void NodeCanopen402Driver<NODETYPE>::add_to_master()
{
NodeCanopenProxyDriver<NODETYPE>::add_to_master();
motor_ = std::make_shared<Motor402>(this->lely_driver_, switching_state_);
}
template <class NODETYPE>
void NodeCanopen402Driver<NODETYPE>::handle_init(
const std_srvs::srv::Trigger::Request::SharedPtr request,
std_srvs::srv::Trigger::Response::SharedPtr response)
{
if (this->activated_.load())
{
bool temp = motor_->handleInit();
response->success = temp;
}
}
template <class NODETYPE>
void NodeCanopen402Driver<NODETYPE>::handle_recover(
const std_srvs::srv::Trigger::Request::SharedPtr request,
std_srvs::srv::Trigger::Response::SharedPtr response)
{
if (this->activated_.load())
{
response->success = motor_->handleRecover();
}
}
template <class NODETYPE>
void NodeCanopen402Driver<NODETYPE>::handle_halt(
const std_srvs::srv::Trigger::Request::SharedPtr request,
std_srvs::srv::Trigger::Response::SharedPtr response)
{
if (this->activated_.load())
{
response->success = motor_->handleHalt();
}
}
template <class NODETYPE>
void NodeCanopen402Driver<NODETYPE>::handle_set_mode_position(
const std_srvs::srv::Trigger::Request::SharedPtr request,
std_srvs::srv::Trigger::Response::SharedPtr response)
{
response->success = set_mode_position();
}
template <class NODETYPE>
void NodeCanopen402Driver<NODETYPE>::handle_set_mode_velocity(
const std_srvs::srv::Trigger::Request::SharedPtr request,
std_srvs::srv::Trigger::Response::SharedPtr response)
{
response->success = set_mode_velocity();
}
template <class NODETYPE>
void NodeCanopen402Driver<NODETYPE>::handle_set_mode_cyclic_position(
const std_srvs::srv::Trigger::Request::SharedPtr request,
std_srvs::srv::Trigger::Response::SharedPtr response)
{
response->success = set_mode_cyclic_position();
}
template <class NODETYPE>
void NodeCanopen402Driver<NODETYPE>::handle_set_mode_interpolated_position(
const std_srvs::srv::Trigger::Request::SharedPtr request,
std_srvs::srv::Trigger::Response::SharedPtr response)
{
response->success = set_mode_interpolated_position();
}
template <class NODETYPE>
void NodeCanopen402Driver<NODETYPE>::handle_set_mode_cyclic_velocity(
const std_srvs::srv::Trigger::Request::SharedPtr request,
std_srvs::srv::Trigger::Response::SharedPtr response)
{
response->success = set_mode_cyclic_velocity();
}
template <class NODETYPE>
void NodeCanopen402Driver<NODETYPE>::handle_set_mode_torque(
const std_srvs::srv::Trigger::Request::SharedPtr request,
std_srvs::srv::Trigger::Response::SharedPtr response)
{
response->success = set_mode_torque();
}
template <class NODETYPE>
void NodeCanopen402Driver<NODETYPE>::handle_set_target(
const canopen_interfaces::srv::COTargetDouble::Request::SharedPtr request,
canopen_interfaces::srv::COTargetDouble::Response::SharedPtr response)
{
if (this->activated_.load())
{
auto mode = motor_->getMode();
double target;
if (
(mode == MotorBase::Profiled_Position) or (mode == MotorBase::Cyclic_Synchronous_Position) or
(mode == MotorBase::Interpolated_Position))
{
target = request->target * scale_pos_to_dev_;
}
else if (
(mode == MotorBase::Velocity) or (mode == MotorBase::Profiled_Velocity) or
(mode == MotorBase::Cyclic_Synchronous_Velocity))
{
target = request->target * scale_vel_to_dev_;
}
else
{
target = request->target;
}
response->success = motor_->setTarget(target);
}
}
template <class NODETYPE>
bool NodeCanopen402Driver<NODETYPE>::init_motor()
{
if (this->activated_.load())
{
bool temp = motor_->handleInit();
return temp;
}
else
{
RCLCPP_INFO(this->node_->get_logger(), "Initialisation failed.");
return false;
}
}
template <class NODETYPE>
bool NodeCanopen402Driver<NODETYPE>::recover_motor()
{
if (this->activated_.load())
{
return motor_->handleRecover();
}
else
{
return false;
}
}
template <class NODETYPE>
bool NodeCanopen402Driver<NODETYPE>::halt_motor()
{
if (this->activated_.load())
{
return motor_->handleHalt();
}
else
{
return false;
}
}
template <class NODETYPE>
bool NodeCanopen402Driver<NODETYPE>::set_operation_mode(uint16_t mode)
{
if (this->activated_.load())
{
return motor_->enterModeAndWait(mode);
}
return false;
}
template <class NODETYPE>
bool NodeCanopen402Driver<NODETYPE>::set_mode_position()
{
if (this->activated_.load())
{
if (motor_->getMode() != MotorBase::Profiled_Position)
{
return motor_->enterModeAndWait(MotorBase::Profiled_Position);
}
else
{
return false;
}
}
else
{
return false;
}
}
template <class NODETYPE>
bool NodeCanopen402Driver<NODETYPE>::set_mode_interpolated_position()
{
if (this->activated_.load())
{
if (motor_->getMode() != MotorBase::Interpolated_Position)
{
return motor_->enterModeAndWait(MotorBase::Interpolated_Position);
}
else
{
return false;
}
}
else
{
return false;
}
}
template <class NODETYPE>
bool NodeCanopen402Driver<NODETYPE>::set_mode_velocity()
{
if (this->activated_.load())
{
if (motor_->getMode() != MotorBase::Profiled_Velocity)
{
return motor_->enterModeAndWait(MotorBase::Profiled_Velocity);
}
else
{
return false;
}
}
else
{
return false;
}
}
template <class NODETYPE>
bool NodeCanopen402Driver<NODETYPE>::set_mode_cyclic_position()
{
if (this->activated_.load())
{
if (motor_->getMode() != MotorBase::Cyclic_Synchronous_Position)
{
return motor_->enterModeAndWait(MotorBase::Cyclic_Synchronous_Position);
}
else
{
return false;
}
}
else
{
return false;
}
}
template <class NODETYPE>
bool NodeCanopen402Driver<NODETYPE>::set_mode_cyclic_velocity()
{
if (this->activated_.load())
{
if (motor_->getMode() != MotorBase::Cyclic_Synchronous_Velocity)
{
return motor_->enterModeAndWait(MotorBase::Cyclic_Synchronous_Velocity);
}
else
{
return false;
}
}
else
{
return false;
}
}
template <class NODETYPE>
bool NodeCanopen402Driver<NODETYPE>::set_mode_torque()
{
if (this->activated_.load())
{
if (motor_->getMode() != MotorBase::Profiled_Torque)
{
return motor_->enterModeAndWait(MotorBase::Profiled_Torque);
}
else
{
return false;
}
}
else
{
return false;
}
}
template <class NODETYPE>
bool NodeCanopen402Driver<NODETYPE>::set_target(double target)
{
if (this->activated_.load())
{
auto mode = motor_->getMode();
double scaled_target;
if (
(mode == MotorBase::Profiled_Position) or (mode == MotorBase::Cyclic_Synchronous_Position) or
(mode == MotorBase::Interpolated_Position))
{
scaled_target = target * scale_pos_to_dev_;
}
else if (
(mode == MotorBase::Velocity) or (mode == MotorBase::Profiled_Velocity) or
(mode == MotorBase::Cyclic_Synchronous_Velocity))
{
scaled_target = target * scale_vel_to_dev_;
}
else
{
scaled_target = target;
}
// RCLCPP_INFO(this->node_->get_logger(), "Scaled target %f", scaled_target);
return motor_->setTarget(scaled_target);
}
else
{
return false;
}
}
template <class NODETYPE>
void NodeCanopen402Driver<NODETYPE>::diagnostic_callback(
diagnostic_updater::DiagnosticStatusWrapper & stat)
{
this->motor_->handleDiag();
stat.summary(this->diagnostic_collector_->getLevel(), this->diagnostic_collector_->getMessage());
stat.add("device_state", this->diagnostic_collector_->getValue("DEVICE"));
stat.add("nmt_state", this->diagnostic_collector_->getValue("NMT"));
stat.add("emcy_state", this->diagnostic_collector_->getValue("EMCY"));
stat.add("cia402_mode", this->diagnostic_collector_->getValue("cia402_mode"));
stat.add("cia402_state", this->diagnostic_collector_->getValue("cia402_state"));
}
#endif