run_trials.hh

Go to the documentation of this file.

#ifndef _RUN_TRIALS_HH_
#define _RUN_TRIALS_HH_

// Gazebo
#include <gazebo/gazebo_client.hh>
#include <gazebo/gazebo_config.h>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
// I/O streams
#include <iostream>
// Threads
#include <chrono>
#include <condition_variable>
#include <mutex>
#include <thread>
// Open YAML config files
#include "yaml-cpp/yaml.h"

// Custom messages
#include "MessageTypes.hh"
// Grasp representation
#include "Grasp.hh"
// Rest pose utils
#include "RestPose.hh"
// Interface for hand plugin
#include "Interface.hh"
// Tools
#include "object_utils.hh"
// Debug streams
#include "debug.hh"
// Randomiser class
#include "Randomiser.hh"

// GAP

// Custom messages
#include "dr_request.pb.h"
// Domain randomization plugin interface
#include "DRInterface.hh"

typedef std::map<std::string,std::string> Config;

// Topics

#define HAND_REQ_TOPIC      "~/hand"
#define HAND_RES_TOPIC      "~/hand/response"
#define TARGET_REQ_TOPIC    "~/grasp/target"
#define TARGET_RES_TOPIC    "~/grasp/target/response"
#define CONTACT_REQ_TOPIC   "~/grasp/contact"
#define CONTACT_RES_TOPIC   "~/grasp/contact/response"
#define CAMERA_REQ_TOPIC    "~/grasp/rgbd"
#define CAMERA_RES_TOPIC    "~/grasp/rgbd/response"
#define FACTORY_TOPIC       "~/factory"
#define REQUEST_TOPIC       "~/request"

// Type enums

// Target Plugin

#define REQ_GET_POSE   grasp::msgs::TargetRequest::GET_POSE
#define REQ_SET_POSE   grasp::msgs::TargetRequest::SET_POSE
#define REQ_REST_POSE  grasp::msgs::TargetRequest::GET_REST_POSE
#define REQ_RESET      grasp::msgs::TargetRequest::RESET
#define RES_POSE       grasp::msgs::TargetResponse::POSE
#define RES_REST_POSE  grasp::msgs::TargetResponse::REST_POSE

// Camera plugin

#define REQ_CAPTURE    grasp::msgs::CameraRequest::CAPTURE

// Hand plugin

#define POSITION grasp::msgs::Target::POSITION
#define VELOCITY grasp::msgs::Target::VELOCITY

// Message type definitions

typedef grasp::msgs::Hand HandMsg;
typedef const boost::shared_ptr<const grasp::msgs::Hand>
    HandMsgPtr;

typedef grasp::msgs::TargetRequest TargetRequest;
typedef const boost::shared_ptr<const grasp::msgs::TargetRequest>
    TargetRequestPtr;
typedef grasp::msgs::TargetResponse TargetResponse;
typedef const boost::shared_ptr<const grasp::msgs::TargetResponse>
    TargetResponsePtr;

typedef grasp::msgs::CollisionRequest CollisionRequest;
typedef grasp::msgs::ContactRequest ContactRequest;
typedef const boost::shared_ptr<const grasp::msgs::ContactRequest>
    ContactRequestPtr;
typedef grasp::msgs::ContactResponse ContactResponse;
typedef const boost::shared_ptr<const grasp::msgs::ContactResponse>
    ContactResponsePtr;

typedef grasp::msgs::CameraRequest CameraRequest;
typedef const boost::shared_ptr<const grasp::msgs::CameraRequest>
    CameraRequestPtr;
typedef grasp::msgs::CameraResponse CameraResponse;
typedef const boost::shared_ptr<const grasp::msgs::CameraResponse>
    CameraResponsePtr;

//
// Argument parsing and setup
//

const std::string getUsage(const char* argv_0);

void parseArgs(
    int argc,
    char** argv,
    Config & config);

void setupCommunications(
    gazebo::transport::NodePtr & node,
    std::map<std::string, gazebo::transport::PublisherPtr> & pubs,
    std::map<std::string, gazebo::transport::SubscriberPtr> & subs);

//
// File I/O
//

void obtainTargets(std::vector<std::string> & targets,
    const std::string & file_name);

bool importGrasps(const std::string & grasp_cfg_dir,
    const std::string & robot,
    const std::string & object_name,
    std::vector<Grasp> & grasps);

void exportGraspMetrics(const std::string & trials_out_dir,
    const std::string & robot,
    const std::string & object_name,
    const std::vector<Grasp> & grasps);

//
// Gazebo plugin interaction
//

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

void checkHandCollisions(gazebo::transport::PublisherPtr pub,
    const std::string & hand,
    std::vector<std::string> & targets);

void closeFingers(gazebo::transport::PublisherPtr pub,
    double timeout=-1);

void liftHand(gazebo::transport::PublisherPtr pub,
    double timeout=-1);

void resetTarget(gazebo::transport::PublisherPtr pub);

void tryGrasp(
    Grasp & grasp,
    Interface & interface,
    std::map<std::string, gazebo::transport::PublisherPtr> & pubs,
    const std::string & model_name);

// Synchronisation

void waitForTrigger(int timeout=-1);

// Callback functions

void onHandResponse(HandMsgPtr & _msg);

void onTargetResponse(TargetResponsePtr & _msg);

void onContactResponse(ContactResponsePtr & _msg);


#endif