Hello Everyone!

I´m trying to write a custom hardware interface and a custom ROS controller. The hardware interface compiles and works, but when I try to compile the controller I get the error: "class CustomHW_Interface::CustomHI has no member named getHandle".

For this I have been following the only 2 or 3 tutorials existing about this: https://slaterobots.com/blog/5abd8a1ed4442a651de5cb5b/how-to-implement-ros_control-on-a-custom-robot , https://www.youtube.com/watch?v=7BLc18lOFJw and http://wiki.ros.org/ros_control/Tutorials/Create%20your%20own%20hardware%20interface. I have been trying to inherit the method from the HardwareResourceManager and from the ResourceManager classes, with no luck.

Basically, the code that I have looks the same as what appears on the slaterobotics web site:

Robot_hardware.h

#ifndef ROS_CONTROL__ROBOT_HARDWARE_H
#define ROS_CONTROL__ROBOT_HARDWARE_H

#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/robot_hw.h>
#include <joint_limits_interface/joint_limits.h>
#include <joint_limits_interface/joint_limits_interface.h>
#include <joint_limits_interface/joint_limits_rosparam.h>
#include <joint_limits_interface/joint_limits_urdf.h>
#include <controller_manager/controller_manager.h>
#include <boost/scoped_ptr.hpp>
#include <ros/ros.h>

namespace Robot_hardware_interface
{
    /// \brief Hardware interface for a robot
    class RobotHardware : public hardware_interface::RobotHW 
    {
        protected:
            // Interfaces
            hardware_interface::JointStateInterface joint_state_interface_;
            hardware_interface::PositionJointInterface position_joint_interface_;
            hardware_interface::VelocityJointInterface velocity_joint_interface_;
            hardware_interface::EffortJointInterface effort_joint_interface_;

            joint_limits_interface::EffortJointSaturationInterface effort_joint_saturation_interface_;
            joint_limits_interface::EffortJointSoftLimitsInterface effort_joint_limits_interface_;
            joint_limits_interface::PositionJointSaturationInterface position_joint_saturation_interface_;
            joint_limits_interface::PositionJointSoftLimitsInterface position_joint_limits_interface_;
            joint_limits_interface::VelocityJointSaturationInterface velocity_joint_saturation_interface_;
            joint_limits_interface::VelocityJointSoftLimitsInterface velocity_joint_limits_interface_;

            // Custom or available transmissions
            // transmission_interface::RRBOTTransmission rrbot_trans_;
            // std::vector<transmission_interface::SimpleTransmission> simple_trans_;

            // Shared memory
            int num_joints_;
            int joint_mode_; // position, velocity, or effort
            std::vector<std::string> joint_names_;
            std::vector<int> joint_types_;
            std::vector<double> joint_position_;
            std::vector<double> joint_velocity_;
            std::vector<double> joint_effort_;
            std::vector<double> joint_position_command_;
            std::vector<double> joint_velocity_command_;
            std::vector<double> joint_effort_command_;
            std::vector<double> joint_lower_limits_;
            std::vector<double> joint_upper_limits_;
            std::vector<double> joint_effort_limits_;

    }; // class

} // namespace

#endif

Robothardwareinterface.h

#ifndef ROS_CONTROL__ROBOT_HARDWARE_INTERFACE_H
#define ROS_CONTROL__ROBOT_HARDWARE_INTERFACE_H

#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/robot_hw.h>
#include <joint_limits_interface/joint_limits_interface.h>
#include <joint_limits_interface/joint_limits.h>
#include <joint_limits_interface/joint_limits_urdf.h>
#include <joint_limits_interface/joint_limits_rosparam.h>
#include <controller_manager/controller_manager.h>
#include <boost/scoped_ptr.hpp>
#include <ros/ros.h>
#include <ROBOTcpp/ROBOT.h>
#include <Robot_hardware_interface/ROBOT_hardware.h>

using namespace hardware_interface;
using joint_limits_interface::JointLimits;
using joint_limits_interface::SoftJointLimits;
using joint_limits_interface::PositionJointSoftLimitsHandle;
using joint_limits_interface::PositionJointSoftLimitsInterface;


namespace Robot_hardware_interface
{
    static const double POSITION_STEP_FACTOR = 10;
    static const double VELOCITY_STEP_FACTOR = 10;

    class RobotHardwareInterface: public ROBOT_hardware_interface::RobotHardware
    {
        public:
            RobotHardwareInterface(ros::NodeHandle& nh);
            ~RobotHardwareInterface();
            void init();
            void update(const ros::TimerEvent& e);
            void read();
            void write(ros::Duration elapsed_time);

        protected:
            ROBOTcpp::ROBOT ROBOT;
            ros::NodeHandle nh_;
            ros::Timer non_realtime_loop_;
            ros::Duration control_period_;
            ros::Duration elapsed_time_;
            PositionJointInterface positionJointInterface;
            PositionJointSoftLimitsInterface positionJointSoftLimitsInterface;
            double loop_hz_;
            boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;
            double p_error_, v_error_, e_error_;
    };

}

#endif

Robothardwareinterface.cpp

#include <sstream>
#include <Robot_hardware_interface/Robot_hardware_interface.h>
#include <joint_limits_interface/joint_limits_interface.h>
#include <joint_limits_interface/joint_limits.h>
#include <joint_limits_interface/joint_limits_urdf.h>
#include <joint_limits_interface/joint_limits_rosparam.h>
#include <ROBOTcpp/ROBOT.h>

using namespace hardware_interface;
using joint_limits_interface::JointLimits;
using joint_limits_interface::SoftJointLimits;
using joint_limits_interface::PositionJointSoftLimitsHandle;
using joint_limits_interface::PositionJointSoftLimitsInterface;

namespace Robot_hardware_interface
{
    RobotHardwareInterface::RobotHardwareInterface(ros::NodeHandle& nh) : nh_(nh) {
        init();
        controller_manager_.reset(new controller_manager::ControllerManager(this, nh_));
        nh_.param("/Robot/hardware_interface/loop_hz", loop_hz_, 0.1);
        ros::Duration update_freq = ros::Duration(1.0/loop_hz_);
        non_realtime_loop_ = nh_.createTimer(update_freq, &TR1HardwareInterface::update, this);
    }

    RobotHardwareInterface::~RobotHardwareInterface() {

    }

    void RobotHardwareInterface::init() {
        // Get joint names
        nh_.getParam("/Robot/hardware_interface/joints", joint_names_);
        num_joints_ = joint_names_.size();

        // Resize vectors
        joint_position_.resize(num_joints_);
        joint_velocity_.resize(num_joints_);
        joint_effort_.resize(num_joints_);
        joint_position_command_.resize(num_joints_);
        joint_velocity_command_.resize(num_joints_);
        joint_effort_command_.resize(num_joints_);

        // Initialize Controller 
        for (int i = 0; i < num_joints_; ++i) {
            ROBOTcpp::Joint joint = ROBOT.getJoint(joint_names_[i]);

             // Create joint state interface
            JointStateHandle jointStateHandle(joint.name, &joint_position_[i], &joint_velocity_[i], &joint_effort_[i]);
             joint_state_interface_.registerHandle(jointStateHandle);

            // Create position joint interface
            JointHandle jointPositionHandle(jointStateHandle, &joint_position_command_[i]);
            JointLimits limits;
                SoftJointLimits softLimits;
            getJointLimits(joint.name, nh_, limits)
            PositionJointSoftLimitsHandle jointLimitsHandle(jointPositionHandle, limits, softLimits);
            positionJointSoftLimitsInterface.registerHandle(jointLimitsHandle);
            position_joint_interface_.registerHandle(jointPositionHandle);

            // Create effort joint interface
            JointHandle jointEffortHandle(jointStateHandle, &joint_effort_command_[i]);
            effort_joint_interface_.registerHandle(jointEffortHandle);
        }

        registerInterface(&joint_state_interface_);
        registerInterface(&position_joint_interface_);
        registerInterface(&effort_joint_interface_);
        registerInterface(&positionJointSoftLimitsInterface);
    }

    void RobotHardwareInterface::update(const ros::TimerEvent& e) {
        elapsed_time_ = ros::Duration(e.current_real - e.last_real);
        read();
        controller_manager_->update(ros::Time::now(), elapsed_time_);
        write(elapsed_time_);
    }

    void RobotHardwareInterface::read() {
        for (int i = 0; i < num_joints_; i++) {
            joint_position_[i] = ROBOT.getJoint(joint_names_[i]).read();
        }
    }

    void RobotHardwareInterface::write(ros::Duration elapsed_time) {
        positionJointSoftLimitsInterface.enforceLimits(elapsed_time);
        for (int i = 0; i < num_joints_; i++) {
            ROBOT.getJoint(joint_names_[i]).actuate(joint_effort_command_[i]);
        }
    }
}

Robothardwareinterface_node.cpp

#include <Robot_hardware_interface/ROBOT_hardware_interface.h>
#include <ros/callback_queue.h> //wasn´t included on slaterobotics website

int main(int argc, char** argv)
{
    ros::init(argc, argv, "Robot_hardware_interface");
    ros::CallbackQueue ros_queue;

    ros::NodeHandle nh;
    nh.setCallbackQueue(&ros_queue);
    ROBOT_hardware_interface::RobotHardwareInterface rhi(nh);
    rhi.init(); //wasn´t included
    ros::MultiThreadedSpinner spinner(0);
    spinner.spin(&ros_queue);
    return 0;
}

roboteffortcontroller.cpp

#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <Robot_hardware_interface/Robot_hardware_interface.h>
#include <pluginlib/class_list_macros.h>
#include <std_msgs/Float64.h>


namespace Robot_Effort_Controller_ns {


    class RobotEffortController : public controller_interface::Controller<hardware_interface::EffortJointInterface> {


        bool init(Robot_hardware_interface::RobotHardwareInterface *hw, ros::NodeHandle &n) { 
            std::string my_joint;
            assert(hw);
            ROS_INFO("Initializing controller...");


            ROS_INFO("Getting joint name...");

            if (!n.getParam("joint", my_joint)) {
                ROS_ERROR("Unable to find joint name");
                return false;


            }
            ROS_INFO("Getting handle...");

            joint_ = hw->getHandle(my_joint); //Error: no member getHandle.

            this->command_ = joint_.getEffort(); //sets current joint goal to the current joint position
            ROS_INFO("Getting gain...");


            //load gains using parameter server
            if (!n.getParam("gain", gain_)) {
                ROS_ERROR("Could not find the gain parameter value");
                return false;

            }

            sub_command_ = n.subscribe<std_msgs::Float64>("command", 1, &RobotEffortController::ControllerCallback, this);

            return true;
        }


        void update(const ros::Time& time, const ros::Duration& period) {
            double error = command_ - joint_.getEffort();
            double commanded_position = error * gain_;

            joint_.setCommand(commanded_position);

        }


        void ControllerCallback(const std_msgs::Float64ConstPtr &msg) {

            command_ = msg->data;


        }

        void starting(const ros::Time &time) {}

        void stopping(const ros::Time &time) {}


    private:
        hardware_interface::JointHandle joint_;
        double gain_;
        double command_;
        std::string joint_name;
        ros::Subscriber sub_command_;


    };

}
    PLUGINLIB_EXPORT_CLASS(Robot_Effort_Controller_ns::RobotEffortController, controller_interface::ControllerBase);

So, my questions is how can I provide the getHandle method to my custom hardware interface?

Best regards, Ch.