robotiq_3f_gripper_ethercat_node.cpp

Go to the documentation of this file.

// Copyright (c) 2016, Toyota Research Institute. All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:

// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.

// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

#include <robotiq_3f_gripper_control/robotiq_3f_gripper_api.h>
#include <robotiq_3f_gripper_control/robotiq_3f_gripper_ethercat_client.h>
#include <robotiq_ethercat/ethercat_manager.h>
#include <robotiq_3f_gripper_control/robotiq_3f_gripper_hw_interface.h>
#include <controller_manager/controller_manager.h>
#include <ros/ros.h>

// Used to convert seconds elapsed to nanoseconds
static const double BILLION = 1000000000.0;

class GenericHWLoop
{
public:
    GenericHWLoop(
            const ros::NodeHandle& nh,
            boost::shared_ptr<robotiq_3f_gripper_control::Robotiq3FGripperHWInterface> hardware_interface)
        : nh_(nh), name_("generic_hw_control_loop"), hardware_interface_(hardware_interface)
    {
        ROS_DEBUG("creating loop");

        controller_manager_.reset(new controller_manager::ControllerManager(hardware_interface_.get(), nh_));
        ROS_DEBUG("created controller manager");

        ros::NodeHandle rpsnh(nh, name_);

        loop_hz_ = rpsnh.param<double>("loop_hz", 30);
        cycle_time_error_threshold_ = rpsnh.param<double>("cycle_time_error_threshold", 0.1);

        clock_gettime(CLOCK_MONOTONIC, &last_time_);

        ros::Duration desired_update_freq_ = ros::Duration(1 / loop_hz_);
        non_realtime_loop_ = nh_.createTimer(desired_update_freq_, &GenericHWLoop::update, this);
        ROS_DEBUG("created timer");
    }

    void update(const ros::TimerEvent& e)
    {
        clock_gettime(CLOCK_MONOTONIC, &current_time_);
        elapsed_time_ = ros::Duration(current_time_.tv_sec - last_time_.tv_sec
                                      + (current_time_.tv_nsec - last_time_.tv_nsec) / BILLION);
        last_time_ = current_time_;
        ROS_DEBUG_STREAM_THROTTLE_NAMED(1, "GenericHWLoop","Sampled update loop with elapsed time " << elapsed_time_.toSec());

        const double cycle_time_error = (elapsed_time_ - desired_update_freq_).toSec();
        if (cycle_time_error > cycle_time_error_threshold_)
        {
            ROS_WARN_STREAM_NAMED(name_, "Cycle time exceeded error threshold by: "
                                  << cycle_time_error << ", cycle time: " << elapsed_time_
                                  << ", threshold: " << cycle_time_error_threshold_);
        }

        hardware_interface_->read(elapsed_time_);

        controller_manager_->update(ros::Time::now(), elapsed_time_);

        hardware_interface_->write(elapsed_time_);
    }

protected:
    // Startup and shutdown of the internal node inside a roscpp program
    ros::NodeHandle nh_;

    // Name of this class
    std::string name_;

    // Settings
    ros::Duration desired_update_freq_;
    double cycle_time_error_threshold_;

    // Timing
    ros::Timer non_realtime_loop_;
    ros::Duration elapsed_time_;
    double loop_hz_;
    struct timespec last_time_;
    struct timespec current_time_;

    boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;

    boost::shared_ptr<robotiq_3f_gripper_control::Robotiq3FGripperHWInterface> hardware_interface_;
};

int main(int argc, char** argv)
{

    using robotiq_ethercat::EtherCatManager;
    using robotiq_3f_gripper_control::Robotiq3FGripperEtherCatClient;
    ros::init(argc, argv, "robotiq_3f_gripper_node");
    ros::NodeHandle nh;
    ros::NodeHandle pnh("~");

    // NOTE: We run the ROS loop in a separate thread as external calls such
    // as service callbacks to load controllers can block the (main) control loop
    ros::AsyncSpinner spinner(2);
    spinner.start();

    // Parameter names
    std::string ifname;
    int slave_no;
    bool activate;

    nh.param<std::string>("ifname", ifname, "eth0");
    nh.param<int>("slave_number", slave_no, 1);
    nh.param<bool>("activate", activate, true);

    // Start ethercat manager
    EtherCatManager manager(ifname);

    // Create the hw client layer
    boost::shared_ptr<robotiq_3f_gripper_control::Robotiq3FGripperEtherCatClient> ethercat_client
            (new robotiq_3f_gripper_control::Robotiq3FGripperEtherCatClient(manager, slave_no));
    ethercat_client->init(pnh);

    // Create the hw api layer
    boost::shared_ptr<robotiq_3f_gripper_control::Robotiq3FGripperAPI> hw_api
            (new robotiq_3f_gripper_control::Robotiq3FGripperAPI(ethercat_client));

    // Create the hardware interface layer
    boost::shared_ptr<robotiq_3f_gripper_control::Robotiq3FGripperHWInterface> hw_interface
            (new robotiq_3f_gripper_control::Robotiq3FGripperHWInterface(pnh, hw_api));

    ROS_DEBUG("created hw interface");

    // Register interfaces
    hardware_interface::JointStateInterface joint_state_interface;
    hardware_interface::PositionJointInterface position_joint_interface;
    hw_interface->configure(joint_state_interface, position_joint_interface);
    hw_interface->registerInterface(&joint_state_interface);
    hw_interface->registerInterface(&position_joint_interface);

    ROS_DEBUG("registered control interfaces");

    // Start the control loop
    GenericHWLoop control_loop(pnh, hw_interface);
    ROS_INFO("started");

    // Wait until shutdown signal recieved
    ros::waitForShutdown();

    return 0;
}