Program Listing for File synapticon_interface.cpp
↰ Return to documentation for file (synapticon_interface.cpp
)
// Copyright (c) 2025 Elevate Robotics Inc
//
// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated
// documentation files (the "Software"), to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to
// permit persons to whom the Software is furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all copies or substantial portions of the
// Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS
// OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#include "synapticon_ros2_control/synapticon_interface.hpp"
#include <algorithm>
#include <chrono>
#include <hardware_interface/types/hardware_interface_type_values.hpp>
#include <inttypes.h>
#include <rclcpp/rclcpp.hpp>
#define EC_TIMEOUTMON 500
using namespace std::chrono_literals;
namespace synapticon_ros2_control {
namespace {
constexpr char LOG_NAME[] = "synapticon_ros2_control";
constexpr double DEG_TO_RAD = 0.0174533;
constexpr size_t PROFILE_TORQUE_MODE = 4;
constexpr size_t CYCLIC_VELOCITY_MODE = 9;
constexpr size_t CYCLIC_POSITION_MODE = 8;
constexpr double RPM_TO_RAD_PER_S = 0.10472;
constexpr double RAD_PER_S_TO_RPM = 1 / RPM_TO_RAD_PER_S;
unsigned int NORMAL_OPERATION_BRAKES_OFF = 0b00001111;
// Bit 2 (0-indexed) goes to 0 to turn on Quick Stop
unsigned int NORMAL_OPERATION_BRAKES_ON = 0b00001011;
constexpr char EXPECTED_SLAVE_NAME[] = "SOMANET";
} // namespace
hardware_interface::CallbackReturn SynapticonSystemInterface::on_init(
const hardware_interface::HardwareInfo &info) {
if (hardware_interface::SystemInterface::on_init(info) !=
hardware_interface::CallbackReturn::SUCCESS) {
return hardware_interface::CallbackReturn::ERROR;
}
logger_ = std::make_shared<rclcpp::Logger>(
rclcpp::get_logger("synapticon_interface"));
num_joints_ = info_.joints.size();
hw_states_positions_.resize(num_joints_,
std::numeric_limits<double>::quiet_NaN());
hw_states_velocities_.resize(num_joints_,
std::numeric_limits<double>::quiet_NaN());
hw_states_accelerations_.resize(num_joints_,
std::numeric_limits<double>::quiet_NaN());
hw_states_efforts_.resize(num_joints_,
std::numeric_limits<double>::quiet_NaN());
hw_commands_positions_.resize(num_joints_,
std::numeric_limits<double>::quiet_NaN());
hw_commands_velocities_.resize(num_joints_,
std::numeric_limits<double>::quiet_NaN());
hw_commands_efforts_.resize(num_joints_,
std::numeric_limits<double>::quiet_NaN());
hw_commands_quick_stop_.resize(num_joints_,
std::numeric_limits<double>::quiet_NaN());
control_level_.resize(num_joints_, control_level_t::UNDEFINED);
// Atomic deques are difficult to initialize
threadsafe_commands_efforts_.resize(num_joints_);
for (auto &effort : threadsafe_commands_efforts_) {
effort.store(std::numeric_limits<double>::quiet_NaN());
}
threadsafe_commands_velocities_.resize(num_joints_);
for (auto &velocity : threadsafe_commands_velocities_) {
velocity.store(0.0);
}
threadsafe_commands_positions_.resize(num_joints_);
for (auto &position : threadsafe_commands_positions_) {
position.store(std::numeric_limits<double>::quiet_NaN());
}
for (const hardware_interface::ComponentInfo &joint : info_.joints) {
if (!(joint.command_interfaces[0].name ==
hardware_interface::HW_IF_POSITION ||
joint.command_interfaces[0].name ==
hardware_interface::HW_IF_VELOCITY ||
joint.command_interfaces[0].name ==
"quick_stop" ||
joint.command_interfaces[0].name ==
hardware_interface::HW_IF_EFFORT)) {
RCLCPP_FATAL(
get_logger(),
"Joint '%s' has %s command interface. Expected %s, %s, %s, or %s.",
joint.name.c_str(), joint.command_interfaces[0].name.c_str(),
hardware_interface::HW_IF_POSITION,
hardware_interface::HW_IF_VELOCITY,
"quick_stop",
hardware_interface::HW_IF_EFFORT);
return hardware_interface::CallbackReturn::ERROR;
}
if (!(joint.state_interfaces[0].name ==
hardware_interface::HW_IF_POSITION ||
joint.state_interfaces[0].name ==
hardware_interface::HW_IF_VELOCITY ||
joint.state_interfaces[0].name ==
hardware_interface::HW_IF_ACCELERATION ||
joint.state_interfaces[0].name == hardware_interface::HW_IF_EFFORT)) {
RCLCPP_FATAL(
get_logger(),
"Joint '%s' has %s state interface. Expected %s, %s, %s, or %s.",
joint.name.c_str(), joint.state_interfaces[0].name.c_str(),
hardware_interface::HW_IF_POSITION,
hardware_interface::HW_IF_VELOCITY,
hardware_interface::HW_IF_ACCELERATION,
hardware_interface::HW_IF_EFFORT);
return hardware_interface::CallbackReturn::ERROR;
}
}
// A thread to handle ethercat errors
osal_thread_create(&ecat_error_thread_, 128000,
(void *)&SynapticonSystemInterface::ecatCheck,
(void *)&ctime);
// Ethercat initialization
// Define the interface name (e.g. eth0 or eno0) in the ros2_control.xacro
std::string interface_name = info_.hardware_parameters["interface_name"];
int ec_init_status = ec_init(interface_name.c_str());
if (ec_init_status <= 0) {
RCLCPP_FATAL_STREAM(get_logger(),
"Error during initialization of ethercat interface: "
<< ec_init_status);
return hardware_interface::CallbackReturn::ERROR;
}
if (ec_config_init(false) <= 0) {
RCLCPP_FATAL(get_logger(), "No ethercat slaves found!");
ec_close();
return hardware_interface::CallbackReturn::ERROR;
}
ec_config_map(&io_map_);
ec_configdc();
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4);
// Request operational state for all slaves
expected_wkc_ = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
for (int slave_id = 0; slave_id < ec_slavecount; ++slave_id) {
ec_slave[slave_id].state = EC_STATE_OPERATIONAL;
}
// send one valid process data to make outputs in slaves happy
ec_send_processdata();
ec_receive_processdata(EC_TIMEOUTRET);
// request OP state for all slaves
ec_writestate(0);
size_t chk = 200;
// wait for all slaves to reach OP state
do {
ec_send_processdata();
ec_receive_processdata(EC_TIMEOUTRET);
ec_statecheck(0, EC_STATE_OPERATIONAL, 50000);
} while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL));
if (ec_slave[0].state != EC_STATE_OPERATIONAL) {
RCLCPP_FATAL(get_logger(),
"An ethercat slave failed to reach OPERATIONAL state");
return hardware_interface::CallbackReturn::ERROR;
}
// Connect struct pointers to I/O
for (size_t joint_idx = 1; joint_idx < (num_joints_ + 1); ++joint_idx) {
in_somanet_1_.push_back((InSomanet50t *)ec_slave[joint_idx].inputs);
out_somanet_1_.push_back((OutSomanet50t *)ec_slave[joint_idx].outputs);
}
// Read encoder resolution
uint8_t encoder_source;
int size = sizeof(encoder_source);
ec_SDOread(1, 0x2012, 0x09, false, &size, &encoder_source, EC_TIMEOUTRXM);
size = sizeof(encoder_resolution_);
if (encoder_source == 1) {
ec_SDOread(1, 0x2110, 0x03, false, &size, &encoder_resolution_,
EC_TIMEOUTRXM);
} else if (encoder_source == 2) {
ec_SDOread(1, 0x2112, 0x03, false, &size, &encoder_resolution_,
EC_TIMEOUTRXM);
} else {
RCLCPP_FATAL(
get_logger(),
"No encoder configured for position control. Terminating the program");
return hardware_interface::CallbackReturn::ERROR;
}
// Start the control loop, wait for it to reach normal operation mode
somanet_control_thread_ =
std::thread(&SynapticonSystemInterface::somanetCyclicLoop, this,
std::ref(in_normal_op_mode_));
return hardware_interface::CallbackReturn::SUCCESS;
}
hardware_interface::return_type
SynapticonSystemInterface::prepare_command_mode_switch(
const std::vector<std::string> &start_interfaces,
const std::vector<std::string> &stop_interfaces) {
// Prepare for new command modes
std::vector<control_level_t> new_modes = {};
for (const std::string& key : start_interfaces) {
for (std::size_t i = 0; i < info_.joints.size(); i++) {
if (key ==
info_.joints[i].name + "/" + hardware_interface::HW_IF_EFFORT) {
new_modes.push_back(control_level_t::EFFORT);
} else if (key == info_.joints[i].name + "/" +
hardware_interface::HW_IF_VELOCITY) {
new_modes.push_back(control_level_t::VELOCITY);
} else if (key == info_.joints[i].name + "/" +
hardware_interface::HW_IF_POSITION) {
new_modes.push_back(control_level_t::POSITION);
} else if (key == info_.joints[i].name + "/quick_stop") {
new_modes.push_back(control_level_t::QUICK_STOP);
}
}
}
// All joints must be given new command mode at the same time
if (!start_interfaces.empty() && (new_modes.size() != num_joints_)) {
RCLCPP_FATAL(get_logger(),
"All joints must be given a new mode at the same time.");
return hardware_interface::return_type::ERROR;
}
// All joints must have the same command mode
if (!std::all_of(
new_modes.begin() + 1, new_modes.end(),
[&](control_level_t mode) { return mode == new_modes[0]; })) {
RCLCPP_FATAL(get_logger(), "All joints must have the same command mode.");
return hardware_interface::return_type::ERROR;
}
// Stop motion on all relevant joints
for (const std::string& key : stop_interfaces) {
for (std::size_t i = 0; i < num_joints_; i++) {
if (key.find(info_.joints[i].name) != std::string::npos) {
hw_commands_positions_[i] = std::numeric_limits<double>::quiet_NaN();
hw_commands_velocities_[i] = 0;
hw_commands_efforts_[i] = 0;
threadsafe_commands_efforts_[i] =
std::numeric_limits<double>::quiet_NaN();
threadsafe_commands_velocities_[i] = 0;
threadsafe_commands_positions_[i] =
std::numeric_limits<double>::quiet_NaN();
control_level_[i] = control_level_t::UNDEFINED;
}
}
}
for (const std::string& key : start_interfaces) {
for (std::size_t i = 0; i < num_joints_; i++) {
if (key.find(info_.joints[i].name) != std::string::npos) {
if (control_level_[i] != control_level_t::UNDEFINED) {
// Something else is using the joint! Abort!
RCLCPP_FATAL(get_logger(),
"Something else is using the joint. Abort!");
return hardware_interface::return_type::ERROR;
}
control_level_[i] = new_modes[i];
}
}
}
return hardware_interface::return_type::OK;
}
hardware_interface::CallbackReturn SynapticonSystemInterface::on_activate(
const rclcpp_lifecycle::State & /*previous_state*/) {
// Set some default values
for (std::size_t i = 0; i < num_joints_; i++) {
if (std::isnan(hw_states_velocities_[i])) {
hw_states_velocities_[i] = 0;
}
if (std::isnan(hw_states_accelerations_[i])) {
hw_states_accelerations_[i] = 0;
}
if (std::isnan(hw_states_efforts_[i])) {
hw_states_efforts_[i] = 0;
}
hw_commands_positions_[i] = std::numeric_limits<double>::quiet_NaN();
hw_commands_velocities_[i] = 0;
hw_commands_efforts_[i] = 0;
threadsafe_commands_efforts_[i] = std::numeric_limits<double>::quiet_NaN();
threadsafe_commands_velocities_[i] = 0;
threadsafe_commands_positions_[i] =
std::numeric_limits<double>::quiet_NaN();
}
RCLCPP_INFO(get_logger(), "System successfully activated! Control level: %u",
control_level_[0]);
return hardware_interface::CallbackReturn::SUCCESS;
}
hardware_interface::CallbackReturn SynapticonSystemInterface::on_deactivate(
const rclcpp_lifecycle::State & /*previous_state*/) {
for (std::size_t i = 0; i < num_joints_; i++) {
control_level_[i] = control_level_t::UNDEFINED;
hw_commands_velocities_[i] = 0;
hw_commands_efforts_[i] = 0;
threadsafe_commands_efforts_[i] = std::numeric_limits<double>::quiet_NaN();
threadsafe_commands_velocities_[i] = 0;
threadsafe_commands_positions_[i] =
std::numeric_limits<double>::quiet_NaN();
}
return hardware_interface::CallbackReturn::SUCCESS;
}
hardware_interface::return_type
SynapticonSystemInterface::read(const rclcpp::Time & /*time*/,
const rclcpp::Duration & /*period*/) {
for (std::size_t i = 0; i < num_joints_; i++) {
std::lock_guard<std::mutex> lock(in_somanet_mtx_);
// InSomanet50t doesn't include acceleration
hw_states_accelerations_[i] = 0;
hw_states_velocities_[i] = in_somanet_1_[i]->VelocityValue * RPM_TO_RAD_PER_S;
hw_states_positions_[i] = in_somanet_1_[0]->PositionValue * 2 * 3.14159 / encoder_resolution_;
hw_states_efforts_[i] = in_somanet_1_[i]->TorqueValue;
}
return hardware_interface::return_type::OK;
}
hardware_interface::return_type
SynapticonSystemInterface::write(const rclcpp::Time & /*time*/,
const rclcpp::Duration & /*period*/) {
// This function doesn't do much.
// It's taken care of in separate thread, somanet_control_thread_
// Share the commands with somanet control loop in a threadsafe way
for (std::size_t i = 0; i < num_joints_; i++) {
// Torque commands are "per thousand of rated torque"
if (!std::isnan(hw_commands_efforts_[i]))
{
hw_commands_efforts_[i] =
std::clamp(hw_commands_efforts_[i], -1000.0, 1000.0);
threadsafe_commands_efforts_[i] = hw_commands_efforts_[i];
}
if (!std::isnan(hw_commands_velocities_[i]))
{
threadsafe_commands_velocities_[i] = hw_commands_velocities_[i] * RAD_PER_S_TO_RPM;
}
if (!std::isnan(hw_commands_positions_[i]))
{
threadsafe_commands_positions_[i] = hw_commands_positions_[i] * encoder_resolution_ / (2 * 3.14159);
}
}
return hardware_interface::return_type::OK;
}
std::vector<hardware_interface::StateInterface>
SynapticonSystemInterface::export_state_interfaces() {
std::vector<hardware_interface::StateInterface> state_interfaces;
for (std::size_t i = 0; i < num_joints_; i++) {
state_interfaces.emplace_back(hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_POSITION,
&hw_states_positions_[i]));
state_interfaces.emplace_back(hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_VELOCITY,
&hw_states_velocities_[i]));
state_interfaces.emplace_back(hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION,
&hw_states_accelerations_[i]));
state_interfaces.emplace_back(hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_EFFORT,
&hw_states_efforts_[i]));
}
return state_interfaces;
}
std::vector<hardware_interface::CommandInterface>
SynapticonSystemInterface::export_command_interfaces() {
std::vector<hardware_interface::CommandInterface> command_interfaces;
for (std::size_t i = 0; i < num_joints_; i++) {
command_interfaces.emplace_back(hardware_interface::CommandInterface(
info_.joints[i].name, hardware_interface::HW_IF_POSITION,
&hw_commands_positions_[i]));
command_interfaces.emplace_back(hardware_interface::CommandInterface(
info_.joints[i].name, hardware_interface::HW_IF_VELOCITY,
&hw_commands_velocities_[i]));
command_interfaces.emplace_back(hardware_interface::CommandInterface(
info_.joints[i].name, hardware_interface::HW_IF_EFFORT,
&hw_commands_efforts_[i]));
command_interfaces.emplace_back(hardware_interface::CommandInterface(
info_.joints[i].name, "quick_stop",
&hw_commands_quick_stop_[i]));
}
return command_interfaces;
}
SynapticonSystemInterface::~SynapticonSystemInterface() {
// A flag to ecat_error_check_ thread
in_normal_op_mode_ = false;
if (somanet_control_thread_ && somanet_control_thread_->joinable()) {
somanet_control_thread_->join();
}
// Close the ethercat connection
ec_close();
}
OSAL_THREAD_FUNC SynapticonSystemInterface::ecatCheck(void * /*ptr*/) {
int slave;
uint8 currentgroup = 0;
while (1) {
if (in_normal_op_mode_ &&
((wkc_ < expected_wkc_) || ec_group[currentgroup].docheckstate)) {
if (needlf_) {
needlf_ = false;
printf("\n");
}
// one or more slaves are not responding
ec_group[currentgroup].docheckstate = false;
ec_readstate();
for (slave = 1; slave <= ec_slavecount; slave++) {
if (ec_slave[slave].name != EXPECTED_SLAVE_NAME)
{
continue;
}
if ((ec_slave[slave].group == currentgroup) &&
(ec_slave[slave].state != EC_STATE_OPERATIONAL)) {
ec_group[currentgroup].docheckstate = true;
if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR)) {
printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n",
slave);
ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK);
ec_writestate(slave);
} else if (ec_slave[slave].state == EC_STATE_SAFE_OP) {
printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n",
slave);
ec_slave[slave].state = EC_STATE_OPERATIONAL;
ec_writestate(slave);
} else if (ec_slave[slave].state > EC_STATE_NONE) {
if (ec_reconfig_slave(slave, EC_TIMEOUTMON)) {
ec_slave[slave].islost = false;
printf("MESSAGE : slave %d reconfigured\n", slave);
}
} else if (!ec_slave[slave].islost) {
// re-check state
ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET);
if (ec_slave[slave].state == EC_STATE_NONE) {
ec_slave[slave].islost = true;
printf("ERROR : slave %d lost\n", slave);
}
}
}
if (ec_slave[slave].islost) {
if (ec_slave[slave].state == EC_STATE_NONE) {
if (ec_recover_slave(slave, EC_TIMEOUTMON)) {
ec_slave[slave].islost = false;
printf("MESSAGE : slave %d recovered\n", slave);
}
} else {
ec_slave[slave].islost = false;
printf("MESSAGE : slave %d found\n", slave);
}
}
}
if (!ec_group[currentgroup].docheckstate)
printf("OK : all slaves resumed OPERATIONAL.\n");
}
osal_usleep(10000);
}
}
void SynapticonSystemInterface::somanetCyclicLoop(
std::atomic<bool> &in_normal_op_mode) {
std::vector<bool> first_iteration(num_joints_ , true);
while (rclcpp::ok()) {
{
std::lock_guard<std::mutex> lock(in_somanet_mtx_);
ec_send_processdata();
wkc_ = ec_receive_processdata(EC_TIMEOUTRET);
if (wkc_ >= expected_wkc_) {
for (size_t joint_idx = 0; joint_idx < num_joints_; ++joint_idx) {
if (first_iteration.at(joint_idx)) {
// Default to PROFILE_TORQUE_MODE
out_somanet_1_[joint_idx]->OpMode = PROFILE_TORQUE_MODE;
out_somanet_1_[joint_idx]->TorqueOffset = 0;
out_somanet_1_[joint_idx]->TargetTorque = 0;
first_iteration.at(joint_idx) = false;
}
// Fault reset: Fault -> Switch on disabled, if the drive is in fault
// state
if ((in_somanet_1_[joint_idx]->Statusword & 0b0000000001001111) ==
0b0000000000001000)
out_somanet_1_[joint_idx]->Controlword = 0b10000000;
// Shutdown: Switch on disabled -> Ready to switch on
else if ((in_somanet_1_[joint_idx]->Statusword &
0b0000000001001111) == 0b0000000001000000)
{
// If the QUICK_STOP controller is on, don't leave this state
if ((control_level_[joint_idx] != control_level_t::QUICK_STOP) &&
(control_level_[joint_idx] != control_level_t::UNDEFINED))
{
out_somanet_1_[joint_idx]->Controlword = 0b00000110;
}
}
// Switch on: Ready to switch on -> Switched on
else if ((in_somanet_1_[joint_idx]->Statusword &
0b0000000001101111) == 0b0000000000100001)
out_somanet_1_[joint_idx]->Controlword = 0b00000111;
// Enable operation: Switched on -> Operation enabled
else if ((in_somanet_1_[joint_idx]->Statusword &
0b0000000001101111) == 0b0000000000100011)
out_somanet_1_[joint_idx]->Controlword = NORMAL_OPERATION_BRAKES_OFF;
// Normal operation
else if ((in_somanet_1_[joint_idx]->Statusword &
0b0000000001101111) == 0b0000000000100111) {
in_normal_op_mode = true;
if (control_level_[joint_idx] == control_level_t::EFFORT) {
if (!std::isnan(threadsafe_commands_efforts_[joint_idx])) {
out_somanet_1_[joint_idx]->TargetTorque =
threadsafe_commands_efforts_[joint_idx];
out_somanet_1_[joint_idx]->OpMode = PROFILE_TORQUE_MODE;
out_somanet_1_[joint_idx]->TorqueOffset = 0;
out_somanet_1_[joint_idx]->Controlword = NORMAL_OPERATION_BRAKES_OFF;
}
} else if (control_level_[joint_idx] == control_level_t::VELOCITY) {
if (!std::isnan(threadsafe_commands_velocities_[joint_idx])) {
out_somanet_1_[joint_idx]->TargetVelocity =
threadsafe_commands_velocities_[joint_idx];
out_somanet_1_[joint_idx]->OpMode = CYCLIC_VELOCITY_MODE;
out_somanet_1_[joint_idx]->VelocityOffset = 0;
out_somanet_1_[joint_idx]->Controlword = NORMAL_OPERATION_BRAKES_OFF;
}
} else if (control_level_[joint_idx] == control_level_t::POSITION) {
if (!std::isnan(threadsafe_commands_positions_[joint_idx])) {
out_somanet_1_[joint_idx]->TargetPosition = threadsafe_commands_positions_[joint_idx];
out_somanet_1_[joint_idx]->OpMode = CYCLIC_POSITION_MODE;
out_somanet_1_[joint_idx]->VelocityOffset = 0;
out_somanet_1_[joint_idx]->Controlword = NORMAL_OPERATION_BRAKES_OFF;
}
} else if (control_level_[joint_idx] == control_level_t::QUICK_STOP)
{
// Turn the brake on
out_somanet_1_[joint_idx]->OpMode = PROFILE_TORQUE_MODE;
out_somanet_1_[joint_idx]->TorqueOffset = 0;
out_somanet_1_[joint_idx]->Controlword = NORMAL_OPERATION_BRAKES_ON;
} else if (control_level_[joint_idx] == control_level_t::UNDEFINED) {
out_somanet_1_[joint_idx]->OpMode = PROFILE_TORQUE_MODE;
out_somanet_1_[joint_idx]->TorqueOffset = 0;
out_somanet_1_[joint_idx]->Controlword = NORMAL_OPERATION_BRAKES_OFF;
}
}
}
// printf("Processdata cycle %4d , WKC %d ,", i, wkc);
// printf(" Statusword: %X ,", in_somanet_1->Statusword);
// printf(" Op Mode Display: %d ,", in_somanet_1->OpModeDisplay);
// printf(" ActualPos: %" PRId32 " ,\n", in_somanet_1_[0]->PositionValue);
// printf(" DemandPos: %" PRId32 " ,",
// in_somanet_1_[0]->PositionDemandInternalValue); printf(" ActualVel:
// %" PRId32 " ,", in_somanet_1_[0]->VelocityValue);
// printf(" DemandVel: %" PRId32 " ,", in_somanet_1_[0]->VelocityDemandValue);
//printf("ActualTorque: %" PRId32 " ,", in_somanet_1_[0]->TorqueValue);
// printf(" DemandTorque: %" PRId32 " ,", in_somanet_1_[0]->TorqueDemand);
// printf("\n");
// printf(" T:%" PRId64 "\r", ec_DCtime);
needlf_ = true;
}
} // scope of in_somanet_ mutex lock
osal_usleep(5000);
}
}
} // namespace synapticon_ros2_control
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(synapticon_ros2_control::SynapticonSystemInterface,
hardware_interface::SystemInterface)