gen_rest_poses.hh

Go to the documentation of this file.

#ifndef _GEN_REST_POSES_HH_
#define _GEN_REST_POSES_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>
// Sleep
#include <chrono>
#include <thread>
// Open YAML config files
#include "yaml-cpp/yaml.h"

// Custom messages
#include "MessageTypes.hh"
#include "MessageTopics.hh"
// Write rest poses to file
#include "RestPose.hh"
// Tools
#include "object_utils.hh"
// Debug streams
#include "debug.hh"

// Type enums

// Target Plugin

#define REQ_REST_POSE  grasp::msgs::TargetRequest::GET_REST_POSE
#define RES_POSE       grasp::msgs::TargetResponse::POSE
#define RES_REST_POSE  grasp::msgs::TargetResponse::REST_POSE

// Message type definitions

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;

// Functions

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

void parseArgs(
    int argc,
    char** argv,
    std::string & obj_cfg_dir,
    std::string & out_rest_dir);

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

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

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

bool waitingTrigger(std::mutex & mutex, bool & trigger);

void onTargetResponse(TargetResponsePtr & _msg);

void inline waitMs(int delay);

#endif