cleaning_plan_alg.h

Go to the documentation of this file.

// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
// Author 
// All rights reserved.
//
// This file is part of iri-ros-pkg
// iri-ros-pkg is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.
// 
// IMPORTANT NOTE: This code has been generated through a script from the 
// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
// of the scripts. ROS topics can be easly add by using those scripts. Please
// refer to the IRI wiki page for more information:
// http://wikiri.upc.es/index.php/Robotics_Lab

#ifndef _cleaning_plan_alg_h_
#define _cleaning_plan_alg_h_

#include <iri_clean_board/CleaningPlanConfig.h>
#include "mutex.h"

//include cleaning_plan_alg libraries
#include <ellipses_representation.h>

// OpenCV
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>

// PRADA Plan Srv msgs
#include <estirabot_msgs/PointsDistanceMsg.h>
#include <estirabot_msgs/ArrayIndexes.h>

// Pointclouds
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/common.h>
#include <pcl_conversions/pcl_conversions.h>

// geometry_msgs and conversions
#include <tf/transform_datatypes.h>
#include <Eigen/Core>
#include <Eigen/Geometry>


class CleaningPlanAlgorithm
{
  protected:
    CMutex alg_mutex_;
    
    // some data suppositions
    static const int IMAGE_WIDTH = 640;
    static const int IMAGE_HEIGHT = 480;
    
    // private attributes and methods
    uint32_t movements_offset;
    double small_objects_min, small_objects_max;
    bool two_arms_version;
    bool board_plan;
    bool rotate_hand;
    double pose_yaw;
    std::string camera_frame;
    
    // values
    double away_from_surface_distance;
    

    void movePoseAwayFromSurface(geometry_msgs::PoseStamped &pose, std::vector<float> plane_coeffs);
    
    std::vector< cv::Point2f > getPointsSmallClean(cv::RotatedRect ellipse);
    
    std::vector< cv::Point2f > getPointsLastSmallClean(cv::RotatedRect ellipse);
    
    std::vector< cv::Point2f > getPointsStraightClean(cv::RotatedRect ellipse);
    
    std::vector< cv::Point2f > getPointsCircularClean(cv::RotatedRect ellipse);
    
    std::vector< cv::Point2f > getPointsGroupObjects(cv::RotatedRect ellipse);
    
    std::vector< cv::Point2f > getPointsJoinGroups(std::vector<cv::RotatedRect> ellipses);
   
    std::vector< cv::Point2f > getPointsPickUp(cv::RotatedRect ellipse);
        
        std::vector< cv::Point2f > getNoPerceptionPoints(cv::Mat image);
    
    std::vector< cv::Point2f > getPointsMoveToContainer(cv::RotatedRect& ellipse, int max_steps = 1);
    
    
    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);
    
    
    geometry_msgs::Quaternion quaternionFrom3dVector(std::vector<float> plane_coefs, double new_yaw = 0);
    
    
    // HELPER METHODS
    
    inline double getDistance(cv::Point2f p1, cv::Point2f p2)
    {
        // sqrt (a^2 + b^2)
        return sqrt((p1.x - p2.x)*(p1.x - p2.x) + (p1.y - p2.y)*(p1.y - p2.y));
    }
    
    inline double getAngle2D (const Eigen::Vector2f &v1, const Eigen::Vector2f &v2)
    {
        // Compute the actual angle
        double rad = v1.dot (v2) / sqrt (v1.squaredNorm () * v2.squaredNorm ());
        if (rad < -1.0) rad = -1.0;
        if (rad >  1.0) rad = 1.0;
        double res = acos(rad);
        
        /*if (res > M_PI/2)
                res = M_PI - res;
        
        if (rad < 0)
                res = - res;*/
        
        if (res != res) // if isNaN
                res = 0;
        
        return res;
    }
    
    inline double getAngle2DwithAxisX (const cv::Point2f &v1, const cv::Point2f &v2)
    {
        Eigen::Vector2f ev;
        ev[0] = v2.x - v1.x;
        ev[1] = v2.y - v1.y;
        
        if (ev[1] > 0)
                return -getAngle2D(Eigen::Vector2f(-1,0),ev);
        else
                return getAngle2D(Eigen::Vector2f(-1,0),ev);
    }
    
    // WARNING copied from newer PCL libs. Remove when it's available
   inline double
   pointToPlaneDistance (const pcl::PointXYZ &p, const std::vector< float > &plane_coefficients)
   {
     return ( fabs (pointToPlaneDistanceSigned (p, plane_coefficients)) );
   }
   
   inline double
   pointToPlaneDistanceSigned (const pcl::PointXYZ &p, const std::vector< float > &plane_coefficients)
   {
     return ( plane_coefficients[0] * p.x + plane_coefficients[1] * p.y + plane_coefficients[2] * p.z + plane_coefficients[3] );
   }
    
    int32_t getNearestPoint(std::vector< cv::Point2f > points1, std::vector< cv::Point2f > points2);
    

  public:
    typedef iri_clean_board::CleaningPlanConfig Config;

    Config config_;

    CleaningPlanAlgorithm(void);

    void lock(void) { alg_mutex_.enter(); };

    void unlock(void) { alg_mutex_.exit(); };

    bool try_enter(void) { return alg_mutex_.try_enter(); };

    void config_update(Config& new_cfg, uint32_t level=0);

    // here define all cleaning_plan_alg interface methods to retrieve and set
    // the driver parameters

    ~CleaningPlanAlgorithm(void);
    
    
    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);
    
    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);
};

#endif