run_trials.hh File Reference

Performs grasp trials. More...

#include <gazebo/gazebo_client.hh>
#include <gazebo/gazebo_config.h>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <iostream>
#include <chrono>
#include <condition_variable>
#include <mutex>
#include <thread>
#include "yaml-cpp/yaml.h"
#include "MessageTypes.hh"
#include "Grasp.hh"
#include "RestPose.hh"
#include "Interface.hh"
#include "object_utils.hh"
#include "debug.hh"
#include "Randomiser.hh"
#include "dr_request.pb.h"
#include "DRInterface.hh"

Go to the source code of this file.

Macros
#define	HAND_REQ_TOPIC   "~/hand"
	Topic monitored by hand plugin for incoming requests. More...
#define	HAND_RES_TOPIC   "~/hand/response"
	Topic for hand plugin responses. More...
#define	TARGET_REQ_TOPIC   "~/grasp/target"
	Topic monitored by target plugin for incoming requests. More...
#define	TARGET_RES_TOPIC   "~/grasp/target/response"
	Topic for target plugin responses. More...
#define	CONTACT_REQ_TOPIC   "~/grasp/contact"
	Topic monitored by contacts plugin for incoming requests. More...
#define	CONTACT_RES_TOPIC   "~/grasp/contact/response"
	Topic for contacts plugin responses. More...
#define	CAMERA_REQ_TOPIC   "~/grasp/rgbd"
	Topic monitored by camera plugin for incoming requests. More...
#define	CAMERA_RES_TOPIC   "~/grasp/rgbd/response"
	Topic for camera plugin responses. More...
#define	FACTORY_TOPIC   "~/factory"
	Topic for Gazebo factory utility. More...
#define	REQUEST_TOPIC   "~/request"
	Topic for generic Gazebo requests. More...
#define	REQ_GET_POSE   grasp::msgs::TargetRequest::GET_POSE
	Get pose request. More...
#define	REQ_SET_POSE   grasp::msgs::TargetRequest::SET_POSE
	Set pose request. More...
#define	REQ_REST_POSE   grasp::msgs::TargetRequest::GET_REST_POSE
	Update rest pose request. More...
#define	REQ_RESET   grasp::msgs::TargetRequest::RESET
	Reset request. More...
#define	RES_POSE   grasp::msgs::TargetResponse::POSE
	Current pose response. More...
#define	RES_REST_POSE   grasp::msgs::TargetResponse::REST_POSE
	Updated rest pose response. More...
#define	REQ_CAPTURE   grasp::msgs::CameraRequest::CAPTURE
	Request to capture frame. More...
#define	POSITION   grasp::msgs::Target::POSITION
	Position control. More...
#define	VELOCITY   grasp::msgs::Target::VELOCITY
	Velocity control. More...

Typedefs
typedef std::map< std::string, std::string >	Config
	Config dictionary. More...
typedef grasp::msgs::Hand	HandMsg
	Declaration for hand message type. More...
typedef const boost::shared_ptr< const grasp::msgs::Hand >	HandMsgPtr
	Shared pointer declaration for hand message type. More...
typedef grasp::msgs::TargetRequest	TargetRequest
	Declaration for request message type. More...
typedef const boost::shared_ptr< const grasp::msgs::TargetRequest >	TargetRequestPtr
	Shared pointer declaration for request message type. More...
typedef grasp::msgs::TargetResponse	TargetResponse
	Declaration for response message type. More...
typedef const boost::shared_ptr< const grasp::msgs::TargetResponse >	TargetResponsePtr
	Shared pointer declaration for response message type. More...
typedef grasp::msgs::CollisionRequest	CollisionRequest
	Declaration for request aux message type. More...
typedef grasp::msgs::ContactRequest	ContactRequest
	Declaration for request message type. More...
typedef const boost::shared_ptr< const grasp::msgs::ContactRequest >	ContactRequestPtr
	Shared pointer declaration for request message type. More...
typedef grasp::msgs::ContactResponse	ContactResponse
	Declaration for response message type. More...
typedef const boost::shared_ptr< const grasp::msgs::ContactResponse >	ContactResponsePtr
	Shared pointer declaration for response message type. More...
typedef grasp::msgs::CameraRequest	CameraRequest
	Declaration for request message type. More...
typedef const boost::shared_ptr< const grasp::msgs::CameraRequest >	CameraRequestPtr
	Shared pointer declaration for request message type. More...
typedef grasp::msgs::CameraResponse	CameraResponse
	Declaration for response message type. More...
typedef const boost::shared_ptr< const grasp::msgs::CameraResponse >	CameraResponsePtr
	Shared pointer declaration for response message type. More...

Functions
const std::string	getUsage (const char *argv_0)
	Obtains usage string. More...
void	parseArgs (int argc, char **argv, Config &config)
	Parses command-line arguments. More...
void	setupCommunications (gazebo::transport::NodePtr &node, std::map< std::string, gazebo::transport::PublisherPtr > &pubs, std::map< std::string, gazebo::transport::SubscriberPtr > &subs)
	Sets up gazebo communication pubs/subs. More...
void	obtainTargets (std::vector< std::string > &targets, const std::string &file_name)
	Obtain list of models' names in dataset yml. More...
bool	importGrasps (const std::string &grasp_cfg_dir, const std::string &robot, const std::string &object_name, std::vector< Grasp > &grasps)
	Obtain list of grasps in yml files. More...
void	exportGraspMetrics (const std::string &trials_out_dir, const std::string &robot, const std::string &object_name, const std::vector< Grasp > &grasps)
	Export set of metrics to file. More...
void	setPose (gazebo::transport::PublisherPtr pub, ignition::math::Pose3d pose, double timeout=-1)
	Sets hand pose. More...
void	checkHandCollisions (gazebo::transport::PublisherPtr pub, const std::string &hand, std::vector< std::string > &targets)
	Requests collisions in the world. More...
void	closeFingers (gazebo::transport::PublisherPtr pub, double timeout=-1)
	Closes manipulator fingers. More...
void	liftHand (gazebo::transport::PublisherPtr pub, double timeout=-1)
	Lifts robotic manipulator along positive z axis. More...
void	resetTarget (gazebo::transport::PublisherPtr pub)
	Resets target object to rest pose. More...
void	tryGrasp (Grasp &grasp, Interface &interface, std::map< std::string, gazebo::transport::PublisherPtr > &pubs, const std::string &model_name)
	Attempts to grasp object. More...
void	waitForTrigger (int timeout=-1)
	Waits for condition variable. More...
void	onHandResponse (HandMsgPtr &_msg)
	TODO. More...
void	onTargetResponse (TargetResponsePtr &_msg)
	TODO. More...
void	onContactResponse (ContactResponsePtr &_msg)
	TODO. More...

Detailed Description

Performs grasp trials.

TODO

Author

João Borrego : jsbruglie

Macro Definition Documentation

#define CAMERA_REQ_TOPIC   "~/grasp/rgbd"

Topic monitored by camera plugin for incoming requests.

#define CAMERA_RES_TOPIC   "~/grasp/rgbd/response"

Topic for camera plugin responses.

#define CONTACT_REQ_TOPIC   "~/grasp/contact"

Topic monitored by contacts plugin for incoming requests.

#define CONTACT_RES_TOPIC   "~/grasp/contact/response"

Topic for contacts plugin responses.

#define FACTORY_TOPIC   "~/factory"

Topic for Gazebo factory utility.

#define HAND_REQ_TOPIC   "~/hand"

Topic monitored by hand plugin for incoming requests.

#define HAND_RES_TOPIC   "~/hand/response"

Topic for hand plugin responses.

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

Position control.

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

Request to capture frame.

#define REQ_GET_POSE   grasp::msgs::TargetRequest::GET_POSE

Get pose request.

#define REQ_RESET   grasp::msgs::TargetRequest::RESET

Reset request.

#define REQ_REST_POSE   grasp::msgs::TargetRequest::GET_REST_POSE

Update rest pose request.

#define REQ_SET_POSE   grasp::msgs::TargetRequest::SET_POSE

Set pose request.

#define REQUEST_TOPIC   "~/request"

Topic for generic Gazebo requests.

#define RES_POSE   grasp::msgs::TargetResponse::POSE

Current pose response.

#define RES_REST_POSE   grasp::msgs::TargetResponse::REST_POSE

Updated rest pose response.

#define TARGET_REQ_TOPIC   "~/grasp/target"

Topic monitored by target plugin for incoming requests.

#define TARGET_RES_TOPIC   "~/grasp/target/response"

Topic for target plugin responses.

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

Velocity control.

Typedef Documentation

typedef grasp::msgs::CameraRequest CameraRequest

Declaration for request message type.

typedef const boost::shared_ptr<const grasp::msgs::CameraRequest> CameraRequestPtr

Shared pointer declaration for request message type.

typedef grasp::msgs::CameraResponse CameraResponse

Declaration for response message type.

typedef const boost::shared_ptr<const grasp::msgs::CameraResponse> CameraResponsePtr

Shared pointer declaration for response message type.

typedef grasp::msgs::CollisionRequest CollisionRequest

Declaration for request aux message type.

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

Config dictionary.

typedef grasp::msgs::ContactRequest ContactRequest

Declaration for request message type.

typedef const boost::shared_ptr<const grasp::msgs::ContactRequest> ContactRequestPtr

Shared pointer declaration for request message type.

typedef grasp::msgs::ContactResponse ContactResponse

Declaration for response message type.

typedef const boost::shared_ptr<const grasp::msgs::ContactResponse> ContactResponsePtr

Shared pointer declaration for response message type.

typedef grasp::msgs::Hand HandMsg

Declaration for hand message type.

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

Shared pointer declaration for hand message type.

typedef grasp::msgs::TargetRequest TargetRequest

Declaration for request message type.

typedef const boost::shared_ptr<const grasp::msgs::TargetRequest> TargetRequestPtr

Shared pointer declaration for request message type.

typedef grasp::msgs::TargetResponse TargetResponse

Declaration for response message type.

typedef const boost::shared_ptr<const grasp::msgs::TargetResponse> TargetResponsePtr

Shared pointer declaration for response message type.

Function Documentation

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

Requests collisions in the world.

Parameters

pub	Publisher to contact topic
target	The target object name
hand	The hand model name

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

Closes manipulator fingers.

Parameters

pub	Publisher to hand topic
timeout	Timeout in seconds

Warning

Applies force directly

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

Export set of metrics to file.

Parameters

trials_out_dir	Output directory
robot	Target robot name
object_name	Target object name
grasps	Set of grasps to export to file

const std::string getUsage

(

const char *

argv_0

)

Obtains usage string.

Parameters

argv_0

Name of the executable

Returns

String with command-line usage

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

Obtain list of grasps in yml files.

Parameters

grasp_cfg_dir	Directory for grasp files
robot	Target robot
object_name	Target object name
grasps	Output imported grasps

Returns

Whether import was successful

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

Lifts robotic manipulator along positive z axis.

Parameters

pub	Publisher to hand topic
timeout	Timeout in seconds

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

Obtain list of models' names in dataset yml.

Parameters

targets	Output list of model names
file_name	Input dataset config yml

Obtain list of models' names in dataset yml.

void onContactResponse

(

ContactResponsePtr &

_msg

)

TODO.

void onHandResponse

(

HandMsgPtr &

_msg

)

TODO.

void onTargetResponse

(

TargetResponsePtr &

_msg

)

TODO.

TODO.

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

Parses command-line arguments.

Parameters

argc	Argument count
argv	Arguments
config	Configuration YAML node

void resetTarget

(

gazebo::transport::PublisherPtr

pub

)

Resets target object to rest pose.

Parameters

pub	Publisher to target's topic

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

Sets hand pose.

Parameters

pub	Publisher to hand's topic
pose	New hand pose

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

Sets up gazebo communication pubs/subs.

Parameters

node	Gazebo communication node pointer
pubs	Resulting map of publishers
subs	Resulting map of subscribers

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

Attempts to grasp object.

Parameters

grasp	The grasp configuration
pubs	Map of publishers
model_name	Target object model name

void waitForTrigger

(

int

timeout = -1

)

Waits for condition variable.

Parameters

timeout

Timeout value in milliseconds