/gpio_command_controller/gpio_states topic show .nan

[ros2torial]

Hi,

I created a custom plugin for omron_nxecc202 + nxid5442 + nxod5256 setup by following the example plugins. After running the controller when I echo _/gpio_command_controller/gpiostates topic the ouput is an array of .nan. Can you please tell what is wrong here?

#include "ethercat_interface/ec_slave.hpp"
#include "ethercat_plugins/commondefs.hpp"
#include <iostream>

namespace ethercat_plugins
{

class Omron_NX_ECC202_CUSTOM : public ethercat_interface::EcSlave
{
public:
    Omron_NX_ECC202_CUSTOM() : EcSlave(0x00000083, 0x000000a6) {}
    virtual ~Omron_NX_ECC202_CUSTOM() {}

    virtual void processData(size_t index, uint16_t* domain_address){

        switch(index){
            case 0:
            {
                uint16_t digital_outputs_;
                digital_outputs_ = 0;
                for(auto i=0ul;i<16;i++){
                    if(cii_dio_[i] >= 0){
                        if(!std::isnan(command_interface_ptr_->at(cii_dio_[i]))){
                            write_data_[i] = (command_interface_ptr_->at(cii_dio_[i])) ? true : false;
                            if(sii_dio_[i] >= 0)
                                state_interface_ptr_->at(sii_dio_[i]) = command_interface_ptr_->at(cii_dio_[i]);
                        }
                    }
                }
                for(int j=0; j<16; j++)
                    digital_outputs_ += ( write_data_[j] << j );
                EC_WRITE_U16(domain_address , digital_outputs_);
                break;
            }
            case 1:
            {
                uint16_t data = 0;
                data = EC_READ_U16(domain_address);
                bool digital_inputs_[16];
                // for(int j=0; j<16; j++)
                //     digital_inputs_[i] = ((data & (0b0000000000000001 << i)) != 0); // bit 0
                digital_inputs_[0] = ((data & 0b0000000000000001) != 0); // bit 0
                digital_inputs_[1] = ((data & 0b0000000000000010) != 0); // bit 1
                digital_inputs_[2] = ((data & 0b0000000000000100) != 0); // bit 2
                digital_inputs_[3] = ((data & 0b0000000000001000) != 0); // bit 3
                digital_inputs_[4] = ((data & 0b0000000000010000) != 0); // bit 4
                digital_inputs_[5] = ((data & 0b0000000000100000) != 0); // bit 5
                digital_inputs_[6] = ((data & 0b0000000001000000) != 0); // bit 6
                digital_inputs_[7] = ((data & 0b0000000010000000) != 0); // bit 7
                digital_inputs_[8] = ((data & 0b0000000100000000) != 0); // bit 8
                digital_inputs_[9] = ((data & 0b0000001000000000) != 0); // bit 9
                digital_inputs_[10] = ((data & 0b0000010000000000) != 0); // bit 10
                digital_inputs_[11] = ((data & 0b0000100000000000) != 0); // bit 11
                digital_inputs_[12] = ((data & 0b0001000000000000) != 0); // bit 12
                digital_inputs_[13] = ((data & 0b0010000000000000) != 0); // bit 13
                digital_inputs_[14] = ((data & 0b0100000000000000) != 0); // bit 14
                digital_inputs_[15] = ((data & 0b1000000000000000) != 0); // bit 15
                for(auto i=16ul;i<32;i++){
                    if(sii_dio_[i] >= 0)
                        state_interface_ptr_->at(sii_dio_[i]) = digital_inputs_[i-16];
                }
                break;
            }
            default:
                std::cout << "WARNING. Omron pdo index = "<<index <<" out of range." << std::endl;
        }

    }
    virtual const ec_sync_info_t* syncs() { return &syncs_[0]; }
    virtual size_t syncSize() {
        return sizeof(syncs_)/sizeof(ec_sync_info_t);
    }
    virtual const ec_pdo_entry_info_t* channels() {
        return channels_;
    }
    virtual void domains(DomainMap& domains) const {
        domains = domains_;
    }
    virtual bool setupSlave(
                std::unordered_map<std::string, std::string> slave_paramters,
                std::vector<double> * state_interface,
                std::vector<double> * command_interface){

        state_interface_ptr_ = state_interface;
        command_interface_ptr_ = command_interface;
        paramters_ = slave_paramters;

        for(auto index = 0ul; index < 32; index++){
            if(paramters_.find("dio."+std::to_string(index+1))!= paramters_.end()){
                if(index<16)
                {if(paramters_.find("command_interface/"+paramters_["dio."+std::to_string(index+1)]) != paramters_.end())
                    cii_dio_[index] = std::stoi(paramters_["command_interface/"+paramters_["dio."+std::to_string(index+1)]]);}
                if(paramters_.find("state_interface/"+paramters_["dio."+std::to_string(index+1)]) != paramters_.end())
                    sii_dio_[index] = std::stoi(paramters_["state_interface/"+paramters_["dio."+std::to_string(index+1)]]);
            }
        }

        return true;
    }

private:

    int sii_dio_[32] = {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1};
    int cii_dio_[32] = {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1};
    bool write_data_[32] = {false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false};

    ec_pdo_entry_info_t channels_[2] = {
        {0x7022, 0x01, 16}, /* Output */
        {0x6002, 0x01, 16}, /* Input */
    };
    ec_pdo_info_t pdos_[2] = {
        {0x1604, 1, channels_ + 0}, /* RxPDO-Map Channel 2 */
        {0x1a00, 1, channels_ + 1}, /* TxPDO-Map Channel 1 */
    };
    ec_sync_info_t syncs_[5] = {
        {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
        {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
        {2, EC_DIR_OUTPUT, 1, pdos_ + 0, EC_WD_ENABLE},
        {3, EC_DIR_INPUT, 1, pdos_ + 1, EC_WD_DISABLE},
        {0xff}
    };
    DomainMap domains_ = {
        {0, {0,1} }
    };
};

}  // namespace ethercat_plugins

#include <pluginlib/class_list_macros.hpp>

PLUGINLIB_EXPORT_CLASS(ethercat_plugins::Omron_NX_ECC202_CUSTOM, ethercat_interface::EcSlave)

[ros2torial]

Issue resolved by creating generic plugins