ecat_lifecycle.hpp

Go to the documentation of this file.

/******************************************************************************
 *
 *  $Id$
 *
 *  Copyright (C) 2021 Veysi ADIN, UST KIST
 *
 *  This file is part of the IgH EtherCAT master userspace program in the ROS2 environment.
 *
 *  The IgH EtherCAT master userspace program in the ROS2 environment 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; version 2
 *  of the License.
 *
 *  The IgH EtherCAT master userspace program in the ROS2 environment 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 the IgH EtherCAT master userspace program in the ROS environment. If not, see
 *  <http://www.gnu.org/licenses/>.
 *
 *  ---
 *
 *  The license mentioned above concerns the source code only. Using the
 *  EtherCAT technology and brand is only permitted in compliance with the
 *  industrial property and similar rights of Beckhoff Automation GmbH.
 *
 *  Contact information: veysi.adin@kist.re.kr
 *****************************************************************************/
/*****************************************************************************
 * \file  ecat_lifecycle.hpp
 * \brief Ethercat lifecycle node implementation and real-time communication
 *        loop implemented in this file.
 *******************************************************************************/
#include "ecat_node.hpp"
#include "timing.hpp"
/******************************************************************************/
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <rclcpp_lifecycle/lifecycle_publisher.hpp>
/******************************************************************************/
#include "ecat_msgs/msg/gui_button_data.hpp"
#include "ecat_msgs/msg/data_received.hpp"
#include "ecat_msgs/msg/data_sent.hpp"
/******************************************************************************/
#include <rclcpp/strategies/message_pool_memory_strategy.hpp>   // /// Completely static memory allocation strategy for messages.
#include <rclcpp/strategies/allocator_memory_strategy.hpp>
 
#include <rttest/rttest.h>   // To get number of allocation, statistics related memory allocation
 
#include <tlsf_cpp/tlsf.hpp>   // C++ wrapper for Miguel Masmano Tello's implementation of the TLSF memory allocator
// Implements the allocator_traits template
 
/******************************************************************************/ 
using rclcpp::strategies::message_pool_memory_strategy::MessagePoolMemoryStrategy;
using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy;
 
template<typename T = void>
using TLSFAllocator = tlsf_heap_allocator<T>;
 
using namespace rclcpp_lifecycle ;
using namespace EthercatCommunication ; 
namespace EthercatLifeCycleNode
{
class EthercatLifeCycle : public LifecycleNode
{
    public:
        EthercatLifeCycle();
        ~EthercatLifeCycle();
    private:
        node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const State &);
    
        node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const State &);
   
        node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const State &);
    
        node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(const State &);
    
        node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(const State &);
    
        node_interfaces::LifecycleNodeInterface::CallbackReturn on_error(const State &);
 
        rclcpp::TimerBase::SharedPtr timer_;
        LifecyclePublisher<ecat_msgs::msg::DataReceived>::SharedPtr received_data_publisher_;
        LifecyclePublisher<ecat_msgs::msg::DataSent>::SharedPtr     sent_data_publisher_;
        rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr         joystick_subscriber_;
        rclcpp::Subscription<ecat_msgs::msg::GuiButtonData>::SharedPtr  gui_subscriber_;
 
        
        ecat_msgs::msg::DataReceived     received_data_;
        ecat_msgs::msg::DataSent         sent_data_;
        std::unique_ptr<EthercatNode>    ecat_node_;
        
        
        void HandleControlNodeCallbacks(const sensor_msgs::msg::Joy::SharedPtr msg);
 
        int SetComThreadPriorities();
        
        int InitEthercatCommunication() ;
 
        static void *PassCycylicExchange(void *arg);
 
        int  StartEthercatCommunication(); 
 
        void StartPdoExchange(void *instance); 
        
        int GetComState();
        
        void ReadFromSlaves();
        
        int PublishAllData();
        int PublishReceivedData();
        
        void EnableMotors();
        
        void WriteToSlavesVelocityMode();
        
        void WriteToSlavesInPositionMode();
        
        void UpdateVelocityModeParameters();
 
        void UpdateCyclicVelocityModeParameters(); 
 
        void UpdatePositionModeParameters();
        
        void UpdateCyclicPositionModeParameters();
        
        void UpdateMotorStateVelocityMode();
        void UpdateMotorStatePositionMode();
        
        void UpdateCyclicTorqueModeParameters();
        
        void WriteToSlavesInCyclicTorqueMode();
        void HandleGuiNodeCallbacks(const ecat_msgs::msg::GuiButtonData::SharedPtr gui_sub);
 
        int GetDriveState(const int& statusWord);
 
        int EnableDrivers();
 
        int8_t EnableDrivesViaSDO(int index);
        int8_t DisableDrivesViaSDO(int index);
        
        int SetConfigurationParameters();
        void UpdateControlParameters();
    private : 
        pthread_t ethercat_thread_;
        struct sched_param ethercat_sched_param_ = {};
        pthread_attr_t ethercat_thread_attr_;
        int32_t err_;
        uint8_t al_state_ = 0; 
        uint32_t motor_state_[g_kNumberOfServoDrivers];
        uint32_t command_ = 0x004F;
        Controller controller_;
        ecat_msgs::msg::GuiButtonData gui_buttons_status_;
        uint8_t gui_node_data_ = 1;
        uint8_t emergency_status_ = 1 ;
        // Will be used as a parameter for taking timing measurements.
        std::int32_t measurement_time = 0 ; 
        Timing timer_info_ ; 
};
}