matlab_controller.h

Go to the documentation of this file.

#ifndef MATLAB_CONTROLLER_H
#define MATLAB_CONTROLLER_H

#include <ros/ros.h>
#include <Eigen/Core>
#include "sensor_msgs/JointState.h"
#include "geometry_msgs/PoseStamped.h"

#include "visual_servoing/JointMove.h"
#include "visual_servoing/robot_controller.h"

class MatlabController : public RobotController {
    public:
        double max_rate = 0.01;
        MatlabController(std::string robot_ns, ros::NodeHandle nh);

        bool delta_move(const Eigen::VectorXd& delta);
        bool rt_delta_move(const Eigen::VectorXd& delta);
        bool joint_move(const Eigen::VectorXd& target_jnt_state);

        geometry_msgs::PoseStamped get_pose() {
            return last_pose;
        }

        Eigen::VectorXd get_jnt_state();
        Eigen::VectorXd get_used_jnt_state();

        size_t get_dof() {
            return last_jnt_state.position.size();
        }

        std::vector<int> get_used_joints() {
            return mapping;
        }

        void set_used_joints(std::string);
        void set_used_joints(std::vector<int> new_joints);

        void set_target_joint_pose(Eigen::VectorXd target);
        Eigen::VectorXd map_joints(const Eigen::VectorXd& delta);

    private:
        // Arm Services
        ros::ServiceClient joint_move_srvs;

        ros::Subscriber pose_sub;
        ros::Subscriber jnt_sub;

        geometry_msgs::PoseStamped last_pose;
        sensor_msgs::JointState last_jnt_state;

        std::vector<int> mapping{0,1,3,4};

        template <size_t DOF>
        Eigen::VectorXd _map_joints(const Eigen::VectorXd& delta);

        void pose_cb(geometry_msgs::PoseStampedConstPtr pose) {
            last_pose = *pose;
        }

        void jnt_state_cb(sensor_msgs::JointStateConstPtr state) {
            last_jnt_state = *state;
        }
};

#endif // MATLAB_CONTROLLER_H