Interface.hh

Go to the documentation of this file.

#ifndef _INTERFACE_HH_
#define _INTERFACE_HH_

#include <vector>
#include <map>
// Open YAML config files
#include "yaml-cpp/yaml.h"

// Gazebo
#include <gazebo/gazebo_client.hh>
#include <gazebo/gazebo_config.h>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>

// Custom utilities
#include "utils.hh"
// Debug streams
#include "debug.hh"
// Grasp shape representation
#include "GraspShape.hh"
// Custom messages
#include "target.pb.h"
#include "hand.pb.h"

// DEBUG
#include <iostream>

#define HAND_PLUGIN_TOPIC "~/hand"

// Key for exiting program (Esc)
#define KEY_EXIT 27

class Interface
{
    public: static const grasp::msgs::Target_Type FORCE =
        grasp::msgs::Target::FORCE;
    public: static const grasp::msgs::Target_Type POSITION = 
        grasp::msgs::Target::POSITION;
    public: static const grasp::msgs::Target_Type VELOCITY =
        grasp::msgs::Target::VELOCITY;

    private: std::string robot_name;
    private: gazebo::transport::NodePtr node;
    private: gazebo::transport::PublisherPtr pub;
    private: std::vector<std::string> joints;
    private: std::map<std::string, double> state;
    private: std::vector<GraspShape> grasps;
    private: ignition::math::Matrix4d t_base_gripper;

    public: Interface();

    public: bool init(
        const std::string & config_file,
        const std::string & robot);

    // Getters

    public: std::string getRobotName();

    public: ignition::math::Matrix4d getTransformBaseGripper();

    // API

    void setPose(ignition::math::Pose3d pose, double timeout=-1);

    public: void reset(void);

    void openFingers(double timeout=-1,
        bool set_position=false);

    void closeFingers(double timeout=-1,
        bool apply_force=false);

    void raiseHand(double timeout=-1);

    public: void loop(void);

    private: int processKeypress(char key);

    private: void moveJoint(
        const char *joint, double value);

    private: void moveFingers(double value);

    private: void setJoints(
        std::vector<std::string> & joints,
        std::vector<double> & values);
};

#endif