cleaning_plan_alg.cpp

Go to the documentation of this file.

#include "cleaning_plan_alg.h"

using namespace cv;
using namespace std;


CleaningPlanAlgorithm::CleaningPlanAlgorithm(void)
{
        // centimeters to move over the plane to dodge objects
        away_from_surface_distance = -0.1;
}

CleaningPlanAlgorithm::~CleaningPlanAlgorithm(void)
{
}

void CleaningPlanAlgorithm::config_update(Config& new_cfg, uint32_t level)
{
  this->lock();

  this->movements_offset     = new_cfg.movements_offset;
  this->small_objects_min    = new_cfg.small_objects_min;
  this->small_objects_max    = new_cfg.small_objects_max;
  this->pose_yaw             = new_cfg.pose_yaw;
  this->two_arms_version     = new_cfg.two_arms_version;
  this->board_plan           = new_cfg.board_plan;
  this->camera_frame         = new_cfg.camera_frame;
  this->rotate_hand          = new_cfg.rotate_hand;
  
  // save the current configuration
  this->config_=new_cfg;
  
  this->unlock();
}


bool CleaningPlanAlgorithm::getMovementFromAction(std::vector< estirabot_msgs::DirtyArea > dirty_areas,
                          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)
{
        std::vector< Point2f > img_coords;
        bool is_real_movement = true;
        uint8_t secondary_arm_movements = 0;
        double orientation_yaw = this->pose_yaw;        // some movements also set the yaw for the secondary arm
//      // TODO ugly fix calibration
        for (size_t i = 0; i < dirty_areas.size(); i++) {
                dirty_areas[i].ellipse.center.y -= 8;
        }
        RotatedRect ellipse1 = ellipses_rep.getEllipseFromRosDirtyAreas(dirty_areas, target_ellipses.indexes[0]);
        
        // BOARD MOVEMENTS
        if (action.compare("circularClean") == 0) {
                img_coords = getPointsCircularClean(ellipse1);
        }
        else if (action.compare("straightClean") == 0) {
                img_coords = getPointsStraightClean(ellipse1);
        }
        else if (action.compare("smallClean") == 0) {
                if (dirty_areas.size() <= 2)
                        img_coords = getPointsLastSmallClean(ellipse1);
                else
                        img_coords = getPointsSmallClean(ellipse1);
                
        }
        else if (action.compare("move") == 0) { // move nowhere, just mark as moved
                is_real_movement = false;
        }
        
        // SMALL OBJECTS MOVEMENTS
        else if (action.compare("group") == 0) {
                img_coords = getPointsGroupObjects(ellipse1);
        }
        else if ( (action.compare("join") == 0) || (action.compare("joinTrav") == 0) ) {
                // getPointsJoinGroups may rearrange order of the action
                // if ellipse2 gets moved instead of ellipse1, 
                //   then ellipse2 will be changed with ellipse1
                vector<RotatedRect> ellipses;
                ellipses.push_back(ellipse1);
                ellipses.push_back(ellipses_rep.getEllipseFromRosDirtyAreas(dirty_areas, target_ellipses.indexes[1]));
                img_coords = getPointsJoinGroups(ellipses);
        }
        else if (action.compare("join3") == 0) {
                // getPointsJoinGroups may rearrange order of the action
                // if ellipse2 gets moved instead of ellipse1, 
                //   then ellipse2 will be changed to point at ellipse1 position
                vector<RotatedRect> ellipses;
                ellipses.push_back(ellipse1);
                ellipses.push_back(ellipses_rep.getEllipseFromRosDirtyAreas(dirty_areas, target_ellipses.indexes[1]));
                ellipses.push_back(ellipses_rep.getEllipseFromRosDirtyAreas(dirty_areas, target_ellipses.indexes[2]));
                img_coords = getPointsJoinGroups(ellipses);
        }
        else if (action.compare("join4") == 0) {
                // getPointsJoinGroups may rearrange order of the action
                // if ellipse2 gets moved instead of ellipse1, 
                //   then ellipse2 will be changed with ellipse1
                vector<RotatedRect> ellipses;
                ellipses.push_back(ellipse1);
                ellipses.push_back(ellipses_rep.getEllipseFromRosDirtyAreas(dirty_areas, target_ellipses.indexes[1]));
                ellipses.push_back(ellipses_rep.getEllipseFromRosDirtyAreas(dirty_areas, target_ellipses.indexes[2]));
                ellipses.push_back(ellipses_rep.getEllipseFromRosDirtyAreas(dirty_areas, target_ellipses.indexes[3]));
                img_coords = getPointsJoinGroups(ellipses);
        }
        else if (action.compare("pickUp") == 0) {
                if (this->two_arms_version) {
                        img_coords = getPointsPickUp(ellipse1);
                        secondary_arm_movements = 2;
                        //orientation_yaw = ellipse1.angle * M_PI / 180;
                        //orientation_yaw = M_PI/2; // only for secondary arm! TODO
                }
                else {
                        // DEPRECATED compatibility with old plans
                        // ellipse1 position will be updated
                        img_coords = getPointsMoveToContainer(ellipse1);
                }
        }
        else if (action.compare("moveToContainerLong") == 0) {
                img_coords = getPointsMoveToContainer(ellipse1, 1);
        }
        else if (action.compare("moveToContainerShort") == 0) { // DEPRECATED
                img_coords = getPointsMoveToContainer(ellipse1, 1);
        }
        else if ( (action.compare("moveToContainerStraight") == 0) || (action.compare("moveToContainerStraightTrav") == 0)){
                img_coords = getPointsMoveToContainer(ellipse1, 3);
        }
        else if (action.compare("preparePickUp") == 0) {
                // Nothing
                is_real_movement = false;
        }
        else if (action.compare("None") == 0) {
                // TODO
                is_real_movement = false;
        }
        else if (action.compare("NoPerception") == 0) {
                img_coords = getNoPerceptionPoints(ellipses_rep.state_image);
                processedPosesFromImageCoordinates(ellipses_rep, cloud, img_coords, plane_coeffs, secondary_arm_movements, orientation_yaw, poses, poses_img_coords, secondary_arm);
                for (size_t i = 4; i < poses.size(); i=i+4) {
                        this->movePoseAwayFromSurface(poses[i], plane_coeffs); // first point of each movement
                        this->movePoseAwayFromSurface(poses[i-1], plane_coeffs); // last point of each movement
                }
                return true;
        }
        else {
                ROS_ERROR_STREAM("CleaningPlanAlgorithm::getImageCoordsFromAction - ACTION NOT FOUND: " << action);
        }
        
        if (is_real_movement) {
                processedPosesFromImageCoordinates(ellipses_rep, cloud, img_coords, plane_coeffs, secondary_arm_movements, orientation_yaw, poses, poses_img_coords, secondary_arm);
        }
        
        return is_real_movement;
}

// TODO this should be called in the upper function
void CleaningPlanAlgorithm::processedPosesFromImageCoordinates(EllipsesRepresentation& ellipses_rep,
                                                                const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, 
                                                                const std::vector< 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
                                                                )
{
        this->posesFromImageCoordinates(cloud, img_coords, 
                                        plane_coeffs, orientation_yaw, 
                                        poses, poses_img_coords, 
                                        secondary_arm_movements, secondary_arm);
        
        if (not this->board_plan) { // when cleaning small objects
                ROS_DEBUG("CleaningPlan - moving away movements");
                
                // move away first pose of current movement
                this->movePoseAwayFromSurface(poses[poses.size() - img_coords.size() + secondary_arm_movements], plane_coeffs);
                
                // move away last pose of current movement
                this->movePoseAwayFromSurface(poses[poses.size() - 1], plane_coeffs);
                
                if (secondary_arm_movements > 0) {
                        this->movePoseAwayFromSurface(poses[poses.size() - img_coords.size()], plane_coeffs);
                        //this->movePoseAwayFromSurface(poses[poses.size() - img_coords.size() + secondary_arm_movements - 1], plane_coeffs);
                }
        }
        
        ellipses_rep.drawPoints(img_coords);
}


std::vector< Point2f > CleaningPlanAlgorithm::getNoPerceptionPoints(cv::Mat image)
{
        // Moving objects from left to right
        std::vector< Point2f > img_coords;
        int step = 20;
        int left_margin = 25; // leave a margin of 25 pixels on the right to make sure that the cloth picks lentils
        int right_edge = 50; // position in x-axis where the arm will try to move the objects
        
        // auxiliar opencv vars
        cv::Rect bounding_box;
        std::vector<std::vector<Point> > contours;
        std::vector<Point> contours_points;
        std::vector<cv::Vec4i> hierarchy;
        
        // opencv stuff
        
        findContours( image, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE, Point(0, 0) );
        for (size_t i = 0; i < contours.size(); i++)
                for (size_t j = 0; j < contours[i].size(); j++)
                        contours_points.push_back(contours[i][j]);
        bounding_box = boundingRect(contours_points);
        
        // set points every step points
        int y_offset=-10;
        // initial point
        Point2f start_point = Point2f(bounding_box.x - left_margin, bounding_box.y + y_offset);
        Point2f end_point;
        while (y_offset < (bounding_box.height + step)) {
                img_coords.push_back(start_point);
                
                // Every point is repeated so it starts and ends the movement away from the surface
                start_point = Point2f(bounding_box.x - left_margin, bounding_box.y + y_offset);
                // first value is very low so it is farther than the edge of the surface
                // the last surface position will be obtained by posesFromImageCoordinates
                Point2f end_point = Point2f(image.size().width - right_edge, bounding_box.y + y_offset);
                
                img_coords.push_back(start_point);
                img_coords.push_back(end_point);
                img_coords.push_back(end_point);
                
                y_offset+=step;
        }
        // final point
        return img_coords;
}


std::vector< Point2f > CleaningPlanAlgorithm::getPointsSmallClean(RotatedRect ellipse)
{
        ROS_DEBUG("CleaningPlan - Generating points for small move");
        std::vector< Point2f > result;
                
        ROS_DEBUG_STREAM("Center is " << ellipse.center);
        
        result.push_back(ellipse.center);
        
        return result;
}


std::vector< Point2f > CleaningPlanAlgorithm::getPointsLastSmallClean(RotatedRect ellipse)
{
        ROS_DEBUG("CleaningPlan - Generating points for one of the last small moves");
        std::vector< Point2f > result;
        Point2f point;
        
        ROS_DEBUG_STREAM("Center is " << ellipse.center);
        
        point = ellipse.center;
        point.y = point.y + 15;
        
        result.push_back(point);
        
        point = ellipse.center;
        point.y = point.y - 15;
        
        result.push_back(point);
        
        return result;
}


std::vector< Point2f > CleaningPlanAlgorithm::getPointsCircularClean(RotatedRect ellipse)
{
        ROS_DEBUG("CleaningPlan - Generating points for circular move");
        
        std::vector< Point2f > result;
        Point2f firstPoint, secondPoint, thirdPoint, fourthPoint;
        float angle = ellipse.angle;
        if (angle > 180)
                angle -=180;
        // to rads
        angle = (angle / 180) * M_PI;
        float x_inc1, y_inc1, x_inc2, y_inc2;
        
        // TODO ROS_ASSERT(ellipse.size.height > ellipse.size.width);
        
        x_inc1 = sin(angle)  * ellipse.size.height / 2;
        y_inc1 = -cos(angle) * ellipse.size.height / 2;
        x_inc2 = cos(angle)  * ellipse.size.width / 2;
        y_inc2 = sin(angle)  * ellipse.size.width / 2;
        
        firstPoint.x = ellipse.center.x + x_inc1;
        firstPoint.y = ellipse.center.y + y_inc1;
        
        secondPoint.x = ellipse.center.x - x_inc1;
        secondPoint.y = ellipse.center.y - y_inc1;
        
        thirdPoint.x = ellipse.center.x + x_inc2;
        thirdPoint.y = ellipse.center.y + y_inc2;
        
        fourthPoint.x = ellipse.center.x - x_inc2;
        fourthPoint.y = ellipse.center.y - y_inc2;
        
        ROS_DEBUG_STREAM("Center is " << ellipse.center);
        ROS_DEBUG_STREAM("Angle is " << angle);
        ROS_DEBUG_STREAM("Width is " << ellipse.size.width);
        ROS_DEBUG_STREAM("Height is " << ellipse.size.height);
        ROS_DEBUG_STREAM("First point " << firstPoint);
        ROS_DEBUG_STREAM("Second point " << secondPoint);
        ROS_DEBUG_STREAM("Third point " << thirdPoint);
        ROS_DEBUG_STREAM("Fourth point " << fourthPoint);
        
        // Define more points if ellipse is too big
        if (ellipse.size.width * ellipse.size.height > 5000) {
                Point2f point5, point6, point7, point8;
                // rotate M_PI/4 to generate 4 new points
                //if (angle > (M_PI/4))
                        angle -= M_PI/4;
                //else
                //      angle += M_PI/4;
                
                x_inc1 =  sin(angle) * (ellipse.size.height + ellipse.size.width) / 4;
                y_inc1 = -cos(angle) * (ellipse.size.height + ellipse.size.width) / 4;
                x_inc2 =  cos(angle) * (ellipse.size.height + ellipse.size.width) / 4;
                y_inc2 =  sin(angle) * (ellipse.size.height + ellipse.size.width) / 4;
                
                point5.x = ellipse.center.x + x_inc1;
                point5.y = ellipse.center.y + y_inc1;
                
                point6.x = ellipse.center.x - x_inc1;
                point6.y = ellipse.center.y - y_inc1;
                
                point7.x = ellipse.center.x + x_inc2;
                point7.y = ellipse.center.y + y_inc2;
                
                point8.x = ellipse.center.x - x_inc2;
                point8.y = ellipse.center.y - y_inc2;
                
                result.push_back(firstPoint);
                result.push_back(point7);
                result.push_back(thirdPoint);
                result.push_back(point6);
                result.push_back(secondPoint);
                result.push_back(point8);
                result.push_back(fourthPoint);
                result.push_back(point5);
        }
        else {
                result.push_back(firstPoint);
                result.push_back(thirdPoint);
                result.push_back(secondPoint);
                result.push_back(fourthPoint);
        }
        
        // TODO needs current position of the arm
        // We should start cleaning at the nearest point
        //if ( getDistance(current_arm_position, result.front()) > getDistance(current_arm_position, result.back()) )
        //      std::reverse(result.begin(), result.end());
        
        return result;
}


std::vector< Point2f > CleaningPlanAlgorithm::getPointsStraightClean(RotatedRect ellipse)
{
        ROS_DEBUG("CleaningPlan - Generating points for straight move");
        std::vector< Point2f > result;
        Point2f firstPoint, secondPoint;
        float angle = (ellipse.angle / 180) * M_PI;
        float x_inc, y_inc;
        
        // TODO ROS_ASSERT(ellipse.size.height > ellipse.size.width);
        
        x_inc =  sin(angle) * (ellipse.size.height / 2 + this->movements_offset);
        y_inc = -cos(angle) * (ellipse.size.height / 2 + this->movements_offset);
        
        firstPoint.x = ellipse.center.x + x_inc;
        firstPoint.y = ellipse.center.y + y_inc;
        secondPoint.x = ellipse.center.x - x_inc;
        secondPoint.y = ellipse.center.y - y_inc;
        
        ROS_DEBUG_STREAM("Center is " << ellipse.center);
        ROS_DEBUG_STREAM("Angle is " << angle);
        ROS_DEBUG_STREAM("Width is " << ellipse.size.width);
        ROS_DEBUG_STREAM("Height is " << ellipse.size.height);
        ROS_DEBUG_STREAM("First point " << firstPoint.x << "," << firstPoint.y);
        ROS_DEBUG_STREAM("Second point " << secondPoint.x << "," << secondPoint.y);
        
        result.push_back(firstPoint);
        result.push_back(secondPoint);
        
        // TODO needs current position of the arm
        // We should start cleaning at the nearest point
        //if ( getDistance(current_arm_position, result.front()) > getDistance(current_arm_position, result.back()) )
        //      std::reverse(result.begin(), result.end());
        
        return result;
}

std::vector< Point2f > CleaningPlanAlgorithm::getPointsGroupObjects(RotatedRect ellipse)
{
        ROS_DEBUG("CleaningPlan - Generating points for group move");
        // Should be: height > width
        std::vector< Point2f > result;
        Point2f firstPoint, secondPoint;
        float angle = ellipse.angle;
        float x_inc, y_inc; // distance center -> end of the ellipse
        
        if (angle > 180)
                angle -=180;
        angle = (angle / 180) * M_PI; // degrees to rads
        
        // TODO ROS_ASSERT(ellipse.size.height > ellipse.size.width);
        
        // if the ellipse is in the upper side and the movement points up
        if ((ellipse.center.y < (IMAGE_HEIGHT*small_objects_min)) && (angle > 3*M_PI/4))
                angle = angle + M_PI; // make it point down left
        
        // if the ellipse is in the lower side and the movement points down
        if ((ellipse.center.y > (IMAGE_HEIGHT*small_objects_max)) && (angle < M_PI/4))
                angle = angle + M_PI; // make it point up left
        
        // else it is already moving to the left
        
        x_inc = sin(angle) * ellipse.size.height / 2;
        y_inc = -cos(angle) * ellipse.size.height / 2;
        
        firstPoint.x  = ellipse.center.x + x_inc;
        firstPoint.y  = ellipse.center.y + y_inc;
        secondPoint.x = ellipse.center.x - x_inc;
        secondPoint.y = ellipse.center.y - y_inc;
        
        // add offset to the points so the arm touches the table with enough space to move them
        firstPoint.x  = firstPoint.x  + ( this->movements_offset *  sin(angle) );
        secondPoint.x = secondPoint.x + ( this->movements_offset *  sin(angle) );
        firstPoint.y  = firstPoint.y  + ( this->movements_offset * -cos(angle) );
        secondPoint.y = secondPoint.y + ( this->movements_offset * -cos(angle) );
        
        ROS_DEBUG_STREAM("Center is " << ellipse.center);
        ROS_DEBUG_STREAM("Angle is " << angle);
        ROS_DEBUG_STREAM("Width is " << ellipse.size.width);
        ROS_DEBUG_STREAM("Height is " << ellipse.size.height);
        ROS_DEBUG_STREAM("First point " << firstPoint.x << "," << firstPoint.y);
        ROS_DEBUG_STREAM("Second point " << secondPoint.x << "," << secondPoint.y);
        
        result.push_back(firstPoint);
        result.push_back(firstPoint);
        result.push_back(secondPoint);
        
        
        if (this->two_arms_version) {
                // already finished
                // only has to group them
                result.push_back(secondPoint); 
        }
        else {
                // Also move the objects to the edge (left)
                Point2f thirdPoint;
                thirdPoint = secondPoint;
                thirdPoint.x = thirdPoint.x - this->movements_offset;
                result.push_back(thirdPoint);
                result.push_back(thirdPoint);
        }
        
        //if (firstPoint.x < secondPoint.x)
        //      std::reverse(result.begin(), result.end());
        
        return result;
}


std::vector< Point2f > CleaningPlanAlgorithm::getPointsJoinGroups(vector<RotatedRect> ellipses)
{
        std::vector< Point2f > result;
        std::vector< Point2f > firstEllipsePoints, secondEllipsePoints;
        Point2f firstPoint, secondPoint, awayDirection;
        size_t first_ellipse_nearest, last_ellipse_nearest, first_ellipse_farthest;
//      double first_ellipse_size = ellipses.front().size.width * ellipses.front().size.height;
//      double last_ellipse_size = ellipses.back().size.width * ellipses.back().size.height;
        
        ROS_DEBUG("CleaningPlan - Generating points for join move");
        
//      if ((this->two_arms_version) && (3 * first_ellipse_size < last_ellipse_size)) { // first_ellipse <<< last_ellipse
//              // nothing
//      }
//      else if ((this->two_arms_version) && (3 * last_ellipse_size < first_ellipse_size)) { // last_ellipse <<< first_ellipse
//              RotatedRect ellipse_aux;
//              
//              // Swap ellipses positions 
//              //  - changes ellipses values so they will be changed in the original vector
//              for (size_t i = 0; i < floor(ellipses.size() / 2); i++){
//                      ellipse_aux = ellipses.at(i);
//                      ellipses.at(i) = ellipses.at(ellipses.size() - i - 1);
//                      ellipses.at(ellipses.size() - i - 1) = ellipse_aux;
//              }
//      }
//      else if (ellipses.front().center.x > ellipses.back().center.x) {  // Prefer moving to the right (container on the right)
//              RotatedRect ellipse_aux;
//              
//              for (size_t i = 0; i < floor(ellipses.size() / 2); i++){
//                      ellipse_aux = ellipses.at(i);
//                      ellipses.at(i) = ellipses.at(ellipses.size() - i - 1);
//                      ellipses.at(ellipses.size() - i - 1) = ellipse_aux;
//              }
//      }
        // else not needed
        
        for (size_t i = 0; i < (ellipses.size() - 1); i++){
                firstEllipsePoints  = getPointsCircularClean(ellipses.at(i));
                secondEllipsePoints = getPointsCircularClean(ellipses.at(i+1));
                
                first_ellipse_nearest  = getNearestPoint(firstEllipsePoints,  secondEllipsePoints);
                last_ellipse_nearest  = getNearestPoint(secondEllipsePoints, firstEllipsePoints);
                
                if (firstEllipsePoints.size() == 8)
                        first_ellipse_farthest = (first_ellipse_nearest + 4) % 8;
                else
                        first_ellipse_farthest = (first_ellipse_nearest + 2) % 4;
                
                firstPoint = firstEllipsePoints[first_ellipse_farthest];
                secondPoint = firstEllipsePoints[first_ellipse_nearest];
                
                ROS_DEBUG_STREAM("Nearest " << first_ellipse_nearest);
                ROS_DEBUG_STREAM("Farthest " << first_ellipse_farthest);
                
                awayDirection.x = firstPoint.x - secondPoint.x;
                awayDirection.y = firstPoint.y - secondPoint.y;
                ROS_DEBUG_STREAM("Away direction is " << awayDirection);
                awayDirection.x = awayDirection.x / sqrt(awayDirection.x*awayDirection.x + awayDirection.y*awayDirection.y);
                awayDirection.y = awayDirection.y / sqrt(awayDirection.x*awayDirection.x + awayDirection.y*awayDirection.y);
                ROS_DEBUG_STREAM("Away direction is " << awayDirection);
                
                if (i == 0) { // only first movement
                        firstPoint.x = firstPoint.x + awayDirection.x * this->movements_offset;
                        firstPoint.y = firstPoint.y + awayDirection.y * this->movements_offset;
                        
                        result.push_back(firstPoint);
                        result.push_back(firstPoint);
                }
                
                result.push_back(secondPoint);
                result.push_back(secondEllipsePoints[last_ellipse_nearest]);
                
        }
//      result.push_back(ellipses.at(ellipses.size() - 1).center);
        
        // repeat last movement to move away from the surface
        result.push_back(result.back());
        
        return result;
}


std::vector< Point2f > CleaningPlanAlgorithm::getPointsPickUp(RotatedRect ellipse)
{
        ROS_DEBUG("CleaningPlan - Generating points for pick up");
        std::vector< Point2f > result;
        Point2f firstPoint, secondPoint;
        float angle = ellipse.angle;
        float x_inc1, x_inc2, x_biggest;
        
        if (angle > 180)
                angle -=180;
        angle = (angle / 180) * M_PI; // to rads
        
        // TODO ROS_ASSERT(ellipse.size.height > ellipse.size.width);
        
        x_inc1 = abs(sin(angle)  * ellipse.size.height / 2);
        x_inc2 = abs(cos(angle)  * ellipse.size.width / 2);
        x_biggest = max(x_inc1, x_inc2);
        
        /* First 
         * Place secondary arm on the right
         */
        firstPoint.x  = ellipse.center.x + x_biggest + this->movements_offset;
        firstPoint.y  = ellipse.center.y;
        
        result.push_back(firstPoint);
        result.push_back(firstPoint);
        
        /* Second 
         * Place main arm on the left
         */
        firstPoint.x  = ellipse.center.x - x_biggest - this->movements_offset;
        firstPoint.y  = ellipse.center.y;
        secondPoint.x  = ellipse.center.x + x_biggest;
        secondPoint.y  = ellipse.center.y;
        
        result.push_back(firstPoint);
        result.push_back(firstPoint);
        result.push_back(secondPoint);
        result.push_back(secondPoint);
        
        ROS_DEBUG("CleaningPlan - Pick up generated");
        
        return result;
}


std::vector< Point2f > CleaningPlanAlgorithm::getPointsMoveToContainer(RotatedRect& ellipse, int max_steps)
{
        ROS_DEBUG("CleaningPlan - Generating points for move right");
        std::vector< Point2f > result;
        Point2f firstPoint, secondPoint;
        float angle = ellipse.angle;
        float x_inc1, x_inc2, x_biggest;
        float y_inc1, y_inc2, y_biggest;
        if (angle > 180)
                angle -=180;
        angle = (angle / 180) * M_PI; // to rads
        
        // TODO ROS_ASSERT(ellipse.size.height > ellipse.size.width);
        
        x_inc1 = abs(sin(angle)  * ellipse.size.height / 2);
        x_inc2 = abs(cos(angle)  * ellipse.size.width / 2);
        x_biggest = max(x_inc1, x_inc2);
        
        y_inc1 = abs(cos(angle)  * ellipse.size.height / 2);
        y_inc2 = abs(sin(angle)  * ellipse.size.width / 2);
        y_biggest = max(y_inc1, y_inc2);
        
        // if the ellipse is in the upper side -> the movement points up
        if (ellipse.center.y < (IMAGE_HEIGHT*small_objects_min) ) {
                firstPoint.x  = ellipse.center.x;
                firstPoint.y  = ellipse.center.y - (y_biggest + this->movements_offset);
                secondPoint.x = ellipse.center.x;
                secondPoint.y = ellipse.center.y + (y_biggest + this->movements_offset);
                
                result.push_back(firstPoint);
                result.push_back(firstPoint);
                result.push_back(secondPoint);
                result.push_back(secondPoint);
        }
        
        // if the ellipse is in the lower side -> the movement points down
        else if (ellipse.center.y > (IMAGE_HEIGHT*small_objects_max) ) {
                firstPoint.x  = ellipse.center.x;
                firstPoint.y  = ellipse.center.y + (y_biggest + this->movements_offset);
                secondPoint.x = ellipse.center.x;
                secondPoint.y = ellipse.center.y - (y_biggest + this->movements_offset);
                
                result.push_back(firstPoint);
                result.push_back(firstPoint);
                result.push_back(secondPoint);
                result.push_back(secondPoint);
        }
        
        else { // move to the right
                int steps, step_size, max_x, current_x, max_steps_by_distance;
                firstPoint.y  = ellipse.center.y;
                firstPoint.x  = ellipse.center.x - (x_biggest + this->movements_offset);
                
                max_x = IMAGE_WIDTH - 40;
                current_x = firstPoint.x;
                max_steps_by_distance = std::floor((max_x - current_x) / this->movements_offset);
                steps = std::min(max_steps, max_steps_by_distance);
                step_size = std::floor((max_x - current_x) / steps);
                
//              secondPoint.y = ellipse.center.y;
//              secondPoint.x = ellipse.center.x - (x_biggest + (max_multiplier*this->movements_offset));
//              if (secondPoint.x < 10) {
//                      secondPoint.x = 10; // force it to be in the image
//              }
                
                result.push_back(firstPoint);
                result.push_back(firstPoint);
                
                for (int i = 1; i <= steps; i++) {
                        current_x += step_size;
                        secondPoint.x = current_x;
                        secondPoint.y = ellipse.center.y;
                        
                        result.push_back(secondPoint);
                }
                result.push_back(secondPoint);
//              for (int i = 1; i <= steps; i++) {
//                      float new_distance_multiplier = (distance_multiplier / steps) * i;
//                      secondPoint.x = ellipse.center.x - (x_biggest + (new_distance_multiplier*this->movements_offset));
//                      secondPoint.y = ellipse.center.y;
//                      
//                      result.push_back(secondPoint);
//                      result.push_back(secondPoint);
//              }
        }
        
        // update ellipse position
        ellipse.center.x = secondPoint.x;
        ellipse.center.y = secondPoint.y;
        
        ROS_DEBUG("CleaningPlan - Move right generated");
        
        return result;
}


int32_t CleaningPlanAlgorithm::getNearestPoint(std::vector< Point2f > points1, std::vector< Point2f > points2)
{
        double min = 10000;
        int32_t min_idx = -1;
        
        for (size_t i=0; i < points1.size(); i++) {
                for (size_t j=0; j < points2.size(); j++) {
                        double dist = getDistance(points1[i], points2[j]);
                        if (dist < min) {
                                min = dist;
                                min_idx = i;
                        }
                }
        }
        
        ROS_DEBUG_STREAM("Minimun distance is: " << min);
        
        return min_idx;
}


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)
{
        geometry_msgs::PoseStamped a_pose;
        iri_perception_msgs::ImagePoint a_pose_coords;
        pcl::PointXYZ point;
        geometry_msgs::Quaternion orientation;
        
        double prev_yaw_increment = orientation_yaw, yaw_increment = orientation_yaw, diff_yaw;
        
        ROS_DEBUG("CleaningPlan - Transforming coords to poses");
        
        for (size_t i = 0; i < img_coords.size(); i++) {
                if (img_coords[i].x < 0)
                        img_coords[i].x = 0;
                if (img_coords[i].y < 0) 
                        img_coords[i].y = 0;
                if (img_coords[i].x > cloud->width)
                        img_coords[i].x = cloud->width - 1;
                if (img_coords[i].y > cloud->height)
                        img_coords[i].y = cloud->height - 1;
                        
                        
                a_pose.header.stamp = ros::Time::now();
                a_pose.header.frame_id = this->camera_frame;
                
                point = cloud->at(img_coords[i].x,img_coords[i].y);
                
                // TODO check
                if ((not this->board_plan) && (not this->two_arms_version)) {
                        // Quick and ugly fix for getPointsMoveToContainer(RotatedRect ellipse) being always in the table
                        //
                        // Until std::isNan (c++11 feature) reach compilers, we use the Nan propiety
                        // of not returning true when comparing against itself
                        while (((point.x != point.x) || (pointToPlaneDistance(point, plane_coeffs) > 0.02 )) && (img_coords[i].x > (IMAGE_WIDTH/2)) ) { // while ( ((point is NaN) || ( (point not in plane)) && (not infinite loop) ) ) 
                                img_coords[i].x -= 1;
                                ROS_DEBUG("NaN point! Trying to fix...");
                                ROS_DEBUG_STREAM("Coords are now " << img_coords[i].x);
                                ROS_DEBUG_STREAM("Point to plane distance is " << pointToPlaneDistance(point, plane_coeffs));
                                point = cloud->at(img_coords[i].x,img_coords[i].y);
                        }
                }
                
                // REMOVE NaNs
                if (point.x != point.x) {
                        point.x = 0;
                        point.y = 0;
                        point.z = 0;
                }
                
                /* Change orientations on the go
                        * Too many rotations, needs more work
                        */
                yaw_increment = prev_yaw_increment;
                
                if (not this->board_plan && this->rotate_hand) {
                        if ( ((i+1) < img_coords.size()) && (img_coords[i].x != img_coords[i+1].x) ) {
                                yaw_increment = orientation_yaw;
                                yaw_increment += getAngle2DwithAxisX(img_coords[i], img_coords[i+1]);
                        }
                }
                
//                      if (yaw_increment > M_PI)
//                              yaw_increment -= M_PI;
//                      if (yaw_increment < 0)
//                              yaw_increment += M_PI;
                
                diff_yaw = yaw_increment - prev_yaw_increment;
                if (diff_yaw > M_PI/2)
                        yaw_increment -= M_PI;
                else if (diff_yaw < - M_PI/2)
                        yaw_increment += M_PI;
                
                if (yaw_increment > 2*M_PI)
                        yaw_increment -= 2*M_PI;
                if (yaw_increment < 0)
                        yaw_increment += 2*M_PI;
                
                prev_yaw_increment = yaw_increment;
                ROS_DEBUG_STREAM("Yaw movement orientation increments " << yaw_increment);
                
                orientation = quaternionFrom3dVector(plane_coeffs, yaw_increment);
                
                // TODO improve secondary arm movements
                /* if (secondary_arm_movements > 1) {
                        if (i < 2)
                                secondary_arm.push_back(true);
                        if (i==5)
                                secondary_arm.push_back(true);
                }
                */
                if (i < secondary_arm_movements)
                        secondary_arm.push_back(true);
                else
                        secondary_arm.push_back(false);
                
                a_pose.pose.position.x = point.x;
                a_pose.pose.position.y = point.y;
                a_pose.pose.position.z = point.z;
                a_pose.pose.orientation = orientation;
                a_pose_coords.x = img_coords[i].x;
                a_pose_coords.y = img_coords[i].y;
                
                poses.push_back(a_pose);
                poses_coords.push_back(a_pose_coords);
        }
        
        ROS_DEBUG("CleaningPlan - Poses generated");
}


void CleaningPlanAlgorithm::movePoseAwayFromSurface(geometry_msgs::PoseStamped &pose, std::vector<float> plane_coeffs)
{
        pose.pose.position.x += (this->away_from_surface_distance * plane_coeffs[0]);
        pose.pose.position.y += (this->away_from_surface_distance * plane_coeffs[1]);
        pose.pose.position.z += (this->away_from_surface_distance * plane_coeffs[2]);
}


geometry_msgs::Quaternion CleaningPlanAlgorithm::quaternionFrom3dVector(std::vector<float> plane_coefs, double new_yaw)
{
        using namespace Eigen;
        geometry_msgs::Quaternion q_msg;
        
        Vector2f vbase(1,0);
        Vector2f vbase2(0,1);
        Vector2f vroll(plane_coefs[1], plane_coefs[2]);
        Vector2f vpitch(plane_coefs[2], plane_coefs[0]);
        Vector2f vyaw(plane_coefs[0], plane_coefs[1]);
        
        double roll = getAngle2D(vroll,vbase2); // rojo (verde -> azul)
        double pitch = getAngle2D(vpitch,vbase); // verde (azul -> rojo)
        double yaw = 0; //getAngle2D(vyaw, vbase); // azul (rojo -> verde)
        //ROS_DEBUG_STREAM("roll " << roll);
        //ROS_DEBUG_STREAM("pitch " << pitch);
        //ROS_DEBUG_STREAM("yaw" << yaw);
        
        if (plane_coefs[2] * plane_coefs[0] < 0)
                pitch = - pitch;

        if (plane_coefs[2] * plane_coefs[1] > 0)
                roll = - roll;
        
        q_msg = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
        
        Quaternion<float> qe1(q_msg.w, q_msg.x, q_msg.y, q_msg.z);
        Quaternion<float> q_rot(AngleAxis<float>(M_PI + new_yaw, Vector3f(0,0,1)));
        Quaternion<float> q_dest;
        q_dest = qe1 * q_rot;
        q_msg.x = q_dest.x();
        q_msg.y = q_dest.y();
        q_msg.z = q_dest.z();
        q_msg.w = q_dest.w();
        
        return q_msg;
}