dxl_motors_builder.h

Go to the documentation of this file.

//
// Created by sub on 16/10/17.
//

#ifndef ARMADILLO2_HW_DXL_MOTORS_BUILDER_H
#define ARMADILLO2_HW_DXL_MOTORS_BUILDER_H

#include <dxl_interface/dxl_interface.h>
#include <std_srvs/SetBool.h>
#include <ros/ros.h>
#include <ros/package.h>
#include <std_msgs/String.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/posvel_command_interface.h>
#include <boost/thread.hpp>

#define MAX_PING_ERRORS 5
#define MAX_READ_ERRORS 100
#define MAX_WRITE_ERRORS 10
#define DXL_JOINTS_CONFIG_PARAM "dxl_joints_config"
#define SPEC_CONFIG_PARAM "dxl_spec_config"
#define DXL_PROTOCOL_PARAM "~dxl_protocol"
#define DXL_PORT_PARAM "~dxl_port_name"
#define DXL_PORT_BAUD_PARAM "~dxl_port_baudrate"


namespace armadillo2_hw
{

    class DxlMotorsBuilder
    {
    public:

        DxlMotorsBuilder(ros::NodeHandle &nh);
        void registerHandles(hardware_interface::JointStateInterface &joint_state_interface,
                             hardware_interface::PositionJointInterface &position_interface,
                             hardware_interface::PosVelJointInterface &posvel_interface);

        /* read/write for controller use */
        void read();
        void write();

        /* writing to single motor, if this motor was build by dxl_builder */
        void writeToMotor(int motor_id, double position, double velocity);

    private:

        ros::NodeHandle *nh_;

        /* handles */
        std::vector<hardware_interface::JointStateHandle> joint_state_handles_;
        std::vector<hardware_interface::PosVelJointHandle> posvel_handles_;
        std::vector<hardware_interface::JointHandle> pos_handles_;

        int dxl_baudrate_ = 0;
        std::string dxl_port_;
        int failed_reads_ = 0, failed_writes_ = 0;
        float protocol_ = 0;
        bool load_dxl_hw_ = true;
        std::map<uint16_t, dxl::spec> specs_; /* key - model number, value - dxl spec */
        dxl::DxlInterface dxl_interface_;
        XmlRpc::XmlRpcValue dxl_joints_config_, dxl_spec_config_;
        std::vector<dxl::motor> motors_;
        ros::ServiceServer torque_srv_;
        ros::Publisher espeak_pub_;
        /* prevent parallel access to dxl motors */
        boost::mutex comm_mutex_;

        bool torqueServiceCB(std_srvs::SetBool::Request  &req,
                             std_srvs::SetBool::Response &res);

        void fetchParams();
        void openPort();
        void buildMotors();
        void pingMotors();
        bool setTorque(bool flag);
        void loadSpecs();
        /* writing directly to motor hardware */
        void write(std::vector<dxl::motor> &motors);
        void speakMsg(std::string msg, int sleep_time)
        {
            std_msgs::String speak_msg;
            speak_msg.data = msg;
            espeak_pub_.publish(speak_msg);
            if (sleep_time > 0)
                sleep(sleep_time);
        }
    };
}
#endif //ARMADILLO2_HW_DXL_MOTORS_BUILDER_H