IRI ROS Specific Driver Class. More...

#include <cleaning_plan_alg.h>

Public Types
typedef iri_clean_board::CleaningPlanConfig	Config
	define config type
Public Member Functions
	CleaningPlanAlgorithm (void)
	constructor
void	config_update (Config &new_cfg, uint32_t level=0)
	config update
bool	getMovementFromAction (std::vector< estirabot_msgs::DirtyArea > dirty_areas, std::string action, estirabot_msgs::ArrayIndexes target_ellipses, EllipsesRepresentation &ellipses_rep, pcl::PointCloud< pcl::PointXYZ >::Ptr cloud, std::vector< float > plane_coeffs, std::vector< geometry_msgs::PoseStamped > &poses, std::vector< iri_perception_msgs::ImagePoint > &poses_img_coords, std::vector< uint8_t > &secondary_arm)
	Gets a trajectory of image points.
void	lock (void)
	Lock Algorithm.
void	processedPosesFromImageCoordinates (EllipsesRepresentation &ellipses_rep, const pcl::PointCloud< pcl::PointXYZ >::Ptr cloud, const std::vector< cv::Point2f > img_coords, const std::vector< float > plane_coeffs, const uint8_t secondary_arm_movements, const double orientation_yaw, std::vector< geometry_msgs::PoseStamped > &poses, std::vector< iri_perception_msgs::ImagePoint > &poses_img_coords, std::vector< uint8_t > &secondary_arm)
	Gets world coordinates from image coordinates adapted to the problem.
bool	try_enter (void)
	Tries Access to Algorithm.
void	unlock (void)
	Unlock Algorithm.
	~CleaningPlanAlgorithm (void)
	Destructor.
Public Attributes
Config	config_
	config variable
Protected Member Functions
double	getAngle2D (const Eigen::Vector2f &v1, const Eigen::Vector2f &v2)
double	getAngle2DwithAxisX (const cv::Point2f &v1, const cv::Point2f &v2)
double	getDistance (cv::Point2f p1, cv::Point2f p2)
int32_t	getNearestPoint (std::vector< cv::Point2f > points1, std::vector< cv::Point2f > points2)
	Get nearest point from vector1 to vector2.
std::vector< cv::Point2f >	getNoPerceptionPoints (cv::Mat image)
	gets points without using ellipses representation
std::vector< cv::Point2f >	getPointsCircularClean (cv::RotatedRect ellipse)
	gets points for doing a circular clean
std::vector< cv::Point2f >	getPointsGroupObjects (cv::RotatedRect ellipse)
	gets points for doing a group move
std::vector< cv::Point2f >	getPointsJoinGroups (std::vector< cv::RotatedRect > ellipses)
	gets points for doing a join move
std::vector< cv::Point2f >	getPointsLastSmallClean (cv::RotatedRect ellipse)
	gets points for doing one of the last small cleans Cleans better than small clean but takes more movements
std::vector< cv::Point2f >	getPointsMoveToContainer (cv::RotatedRect &ellipse, int max_steps=1)
	gets points for doing a move out (pick up alternative)
std::vector< cv::Point2f >	getPointsPickUp (cv::RotatedRect ellipse)
	gets points for doing a pick up
std::vector< cv::Point2f >	getPointsSmallClean (cv::RotatedRect ellipse)
	gets points for doing a small clean
std::vector< cv::Point2f >	getPointsStraightClean (cv::RotatedRect ellipse)
	gets points for doing a straight clean
void	movePoseAwayFromSurface (geometry_msgs::PoseStamped &pose, std::vector< float > plane_coeffs)
	moves a pose away from the surface Moves the pose away some centimeters from the surface defined by plane_coeffs
double	pointToPlaneDistance (const pcl::PointXYZ &p, const std::vector< float > &plane_coefficients)
double	pointToPlaneDistanceSigned (const pcl::PointXYZ &p, const std::vector< float > &plane_coefficients)
void	posesFromImageCoordinates (pcl::PointCloud< pcl::PointXYZ >::Ptr cloud, std::vector< cv::Point2f > img_coords, std::vector< float > plane_coeffs, double orientation_yaw, std::vector< geometry_msgs::PoseStamped > &poses, std::vector< iri_perception_msgs::ImagePoint > &poses_coords, uint8_t secondary_arm_movements, std::vector< uint8_t > &secondary_arm)
	Gets world coordinates from image coordinates.
geometry_msgs::Quaternion	quaternionFrom3dVector (std::vector< float > plane_coefs, double new_yaw=0)
	Gets quaternion from a plane normal.
Protected Attributes
CMutex	alg_mutex_
	define config type
double	away_from_surface_distance
bool	board_plan
std::string	camera_frame
uint32_t	movements_offset
double	pose_yaw
bool	rotate_hand
double	small_objects_max
double	small_objects_min
bool	two_arms_version
Static Protected Attributes
static const int	IMAGE_HEIGHT = 480
static const int	IMAGE_WIDTH = 640

Detailed Description

IRI ROS Specific Driver Class.

Definition at line 61 of file cleaning_plan_alg.h.

Member Typedef Documentation

typedef iri_clean_board::CleaningPlanConfig CleaningPlanAlgorithm::Config

define config type

Define a Config type with the CleaningPlanConfig. All driver implementations will then use the same variable type Config.

Definition at line 243 of file cleaning_plan_alg.h.

Constructor & Destructor Documentation

CleaningPlanAlgorithm::CleaningPlanAlgorithm ( void )

constructor

In this constructor parameters related to the specific driver can be initalized. Those parameters can be also set in the openDriver() function. Attributes from the main node driver class IriBaseDriver such as loop_rate, may be also overload here.

Definition at line 7 of file cleaning_plan_alg.cpp.

CleaningPlanAlgorithm::~CleaningPlanAlgorithm ( void )

Destructor.

This destructor is called when the object is about to be destroyed.

Definition at line 13 of file cleaning_plan_alg.cpp.

Member Function Documentation

void CleaningPlanAlgorithm::config_update	(	Config &	new_cfg,
		uint32_t	level = `0`
	)

config update

In this function the driver parameters must be updated with the input config variable. Then the new configuration state will be stored in the Config attribute.

Parameters:

new_cfg	the new driver configuration state
level	level in which the update is taken place

Definition at line 17 of file cleaning_plan_alg.cpp.

double CleaningPlanAlgorithm::getAngle2D	(	const Eigen::Vector2f &	v1,
		const Eigen::Vector2f &	v2
	)		`[inline, protected]`

Definition at line 182 of file cleaning_plan_alg.h.

double CleaningPlanAlgorithm::getAngle2DwithAxisX	(	const cv::Point2f &	v1,
		const cv::Point2f &	v2
	)		`[inline, protected]`

Definition at line 202 of file cleaning_plan_alg.h.

double CleaningPlanAlgorithm::getDistance	(	cv::Point2f	p1,
		cv::Point2f	p2
	)		`[inline, protected]`

Definition at line 176 of file cleaning_plan_alg.h.

bool CleaningPlanAlgorithm::getMovementFromAction	(	std::vector< estirabot_msgs::DirtyArea >	dirty_areas,
		std::string	action,
		estirabot_msgs::ArrayIndexes	target_ellipses,
		EllipsesRepresentation &	ellipses_rep,
		pcl::PointCloud< pcl::PointXYZ >::Ptr	cloud,
		std::vector< float >	plane_coeffs,
		std::vector< geometry_msgs::PoseStamped > &	poses,
		std::vector< iri_perception_msgs::ImagePoint > &	poses_img_coords,
		std::vector< uint8_t > &	secondary_arm
	)

Gets a trajectory of image points.

Gets a trajectory of image points that would make the requested move

Parameters:

action	the string identifying the action being processed
target_ellipses	indexes of the ellipses implicated in the action
cloud	pointcloud from which to extract the 3D coordinates
plane_coeffs	plane coefficients for getting the orientation of the plane
poses	vector to which add poses corresponding to the action
poses_img_coords	coordinates corresponding to the poses
secondary_arm	vector that tells if the actions is for the main (false) or the secondary_arm (true)

Returns:: if there is any movement

Definition at line 37 of file cleaning_plan_alg.cpp.

int32_t CleaningPlanAlgorithm::getNearestPoint	(	std::vector< cv::Point2f >	points1,
		std::vector< cv::Point2f >	points2
	)		`[protected]`

Get nearest point from vector1 to vector2.

Get nearest point from vector1 to vector2

Definition at line 705 of file cleaning_plan_alg.cpp.

std::vector< Point2f > CleaningPlanAlgorithm::getNoPerceptionPoints ( cv::Mat image ) [protected]

gets points without using ellipses representation

gets points without using ellipses representation, only a bounding box

Definition at line 194 of file cleaning_plan_alg.cpp.

std::vector< Point2f > CleaningPlanAlgorithm::getPointsCircularClean ( cv::RotatedRect ellipse ) [protected]

gets points for doing a circular clean

Definition at line 276 of file cleaning_plan_alg.cpp.

std::vector< Point2f > CleaningPlanAlgorithm::getPointsGroupObjects ( cv::RotatedRect ellipse ) [protected]

gets points for doing a group move

Small Objects

Definition at line 409 of file cleaning_plan_alg.cpp.

std::vector< Point2f > CleaningPlanAlgorithm::getPointsJoinGroups ( std::vector< cv::RotatedRect > ellipses ) [protected]

gets points for doing a join move

Definition at line 481 of file cleaning_plan_alg.cpp.

std::vector< Point2f > CleaningPlanAlgorithm::getPointsLastSmallClean ( cv::RotatedRect ellipse ) [protected]

gets points for doing one of the last small cleans Cleans better than small clean but takes more movements

Definition at line 254 of file cleaning_plan_alg.cpp.

std::vector< Point2f > CleaningPlanAlgorithm::getPointsMoveToContainer	(	cv::RotatedRect &	ellipse,
		int	max_steps = `1`
	)		`[protected]`

gets points for doing a move out (pick up alternative)

Generates 3D points to move the lentils in ellipse towards a container on the right edge of the table.

Parameters:

ellipse	ellipse representing the lentils to be moved
distance_multiplier	the robot will move the lentils distance_multiplier * movements_offset towards the edge
steps	number of intermediate points generated (will create smoother trajectories)

Definition at line 609 of file cleaning_plan_alg.cpp.

std::vector< Point2f > CleaningPlanAlgorithm::getPointsPickUp ( cv::RotatedRect ellipse ) [protected]

gets points for doing a pick up

Definition at line 563 of file cleaning_plan_alg.cpp.

std::vector< Point2f > CleaningPlanAlgorithm::getPointsSmallClean ( cv::RotatedRect ellipse ) [protected]

gets points for doing a small clean

Definition at line 241 of file cleaning_plan_alg.cpp.

std::vector< Point2f > CleaningPlanAlgorithm::getPointsStraightClean ( cv::RotatedRect ellipse ) [protected]

gets points for doing a straight clean

Definition at line 368 of file cleaning_plan_alg.cpp.

void CleaningPlanAlgorithm::lock ( void ) [inline]

Lock Algorithm.

Locks access to the Algorithm class

Definition at line 268 of file cleaning_plan_alg.h.

void CleaningPlanAlgorithm::movePoseAwayFromSurface	(	geometry_msgs::PoseStamped &	pose,
		std::vector< float >	plane_coeffs
	)		`[protected]`

moves a pose away from the surface Moves the pose away some centimeters from the surface defined by plane_coeffs

Definition at line 843 of file cleaning_plan_alg.cpp.

double CleaningPlanAlgorithm::pointToPlaneDistance	(	const pcl::PointXYZ &	p,
		const std::vector< float > &	plane_coefficients
	)		`[inline, protected]`

Definition at line 216 of file cleaning_plan_alg.h.

double CleaningPlanAlgorithm::pointToPlaneDistanceSigned	(	const pcl::PointXYZ &	p,
		const std::vector< float > &	plane_coefficients
	)		`[inline, protected]`

Definition at line 222 of file cleaning_plan_alg.h.

void CleaningPlanAlgorithm::posesFromImageCoordinates	(	pcl::PointCloud< pcl::PointXYZ >::Ptr	cloud,
		std::vector< cv::Point2f >	img_coords,
		std::vector< float >	plane_coeffs,
		double	orientation_yaw,
		std::vector< geometry_msgs::PoseStamped > &	poses,
		std::vector< iri_perception_msgs::ImagePoint > &	poses_coords,
		uint8_t	secondary_arm_movements,
		std::vector< uint8_t > &	secondary_arm
	)		`[protected]`

Gets world coordinates from image coordinates.

Gets poseStampeds from image coordinates using the input pointcloud

Definition at line 726 of file cleaning_plan_alg.cpp.

void CleaningPlanAlgorithm::processedPosesFromImageCoordinates	(	EllipsesRepresentation &	ellipses_rep,
		const pcl::PointCloud< pcl::PointXYZ >::Ptr	cloud,
		const std::vector< cv::Point2f >	img_coords,
		const std::vector< float >	plane_coeffs,
		const uint8_t	secondary_arm_movements,
		const double	orientation_yaw,
		std::vector< geometry_msgs::PoseStamped > &	poses,
		std::vector< iri_perception_msgs::ImagePoint > &	poses_img_coords,
		std::vector< uint8_t > &	secondary_arm
	)

Gets world coordinates from image coordinates adapted to the problem.

Gets poseStampeds from image coordinates using the input pointcloud adapted to the problem. When cleaning beans, moves the first and last movement away from the surface. Also shows movements in ellipses_rep.

Definition at line 159 of file cleaning_plan_alg.cpp.