wam_actions.h

Go to the documentation of this file.

#ifndef _wam_actions_h_
#define _wam_actions_h_

#include "ros/this_node.h"
#include "ros/names.h"
#include "ros/node_handle.h"
#include "ros/rate.h"

// [publisher subscriber headers]

// [service client headers]
#include "iri_wam_common_msgs/bhand_cmd.h"
#include "iri_wam_common_msgs/pose_move.h"
#include "iri_wam_common_msgs/wamaction.h"
#include "iri_wam_common_msgs/compute_obj_grasp_pose.h"

#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include "tf/tfMessage.h"
// [action server client headers]

#include "mutex.h"

#define NUMPOSES 3
#define AZONEMSG 1
#define BZONEMSG 2
#define ANYZONEMSG 0
#define AZONE 1000
#define BZONE 2000
#define ANYZONE 0

#define ALLFILTERS 0

#define NOX 0.5
#define NOY 0
#define NOZ 0

enum {
    //remember to change the other .h enums (dummy_pomdp)!
    TAKEHIGH,
    TAKELOW,
    CHANGEPILE,
    FOLDLOW,
    SPREAD,
    FLIP,
    SHAKE,
    FOLDHIGH,
    MOVEAWAY,
    SIDEGRASP,
    DUMMY
};

enum {
    MAXWRINKLE,
    MAXHEIGHT
};

enum {
    STRAIGHT,
    ISOMETRIC,
    PEG
};

class WamActions
{
  private: 
    CMutex tf_broadcaster_mutex;
    ros::NodeHandle node_handle_;
    geometry_msgs::Pose sampleposes[NUMPOSES];
    tf::TransformBroadcaster tf_br;
    tf::TransformListener tf_grasp;

    std::string wam0_;

    // [publisher attributes]
    tf::Transform transform_grasping_point;

    // [subscriber attributes]

    // [service attributes]
    ros::ServiceServer wam_action_server;
    bool wam_actionCallback(iri_wam_common_msgs::wamaction::Request &req, iri_wam_common_msgs::wamaction::Response &res);

    // [client attributes]
    ros::ServiceClient obj_filter_client;
    iri_wam_common_msgs::compute_obj_grasp_pose obj_filter_srv;
    ros::ServiceClient barrett_hand_cmd_client;
    iri_wam_common_msgs::bhand_cmd barrett_hand_cmd_srv;
    ros::ServiceClient pose_move_client;
    iri_wam_common_msgs::pose_move pose_move_srv;

    // [action server attributes]

    // [action client attributes]
    bool check_box(double x, double y, double z);
    bool fit_box(double& x, double& y, double& z);
    bool go_to(double x, double y, double z); 
    bool go_to_kinect(); 

    int destination_counter;
    void get_destination(int destination_zone, double& x, double& y, double& z);


  public:
    WamActions();

    void mainLoop(void);

    //new more general purpose actions
    bool requestGraspingPoint(double &x, double &y, double &z, int zone = ANYZONE, int filterID = ALLFILTERS);
    bool requestGraspingPointMaxHeight(double &x, double &y, double &z, int zone = ANYZONE, int filterID = ALLFILTERS);
    bool take3D(double x, double y, double z);
    bool take3Dside(double x, double y, double z);
    bool drop();
    bool drop(double x, double y, double z);
    bool isograsp(); 
    bool peggrasp();

    //take related commands
    bool stretch();
    bool fold();
    bool fromAtoB();
    bool fromBtoA();
    bool flip();
    bool shake();


};
#endif