cleaning_plan_alg_node.cpp

Go to the documentation of this file.

#include "cleaning_plan_alg_node.h"

using namespace std;
using namespace cv;

CleaningPlanAlgNode::CleaningPlanAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<CleaningPlanAlgorithm>()
{
        //init class attributes if necessary
        //this->loop_rate_ = 2;//in [Hz]

        // [init publishers]
        this->image_plan_out_publisher_ = this->public_node_handle_.advertise<sensor_msgs::Image>("image_plan_out", 1);
        this->visualization_markers_publisher_ = this->public_node_handle_.advertise<visualization_msgs::MarkerArray>("visualization_markers", 1);
        
        // [init subscribers]
        
        // [init services]
        this->representation_to_string_server_ = this->public_node_handle_.advertiseService("representation_to_string", &CleaningPlanAlgNode::representation_to_stringCallback, this);
        this->get_dirty_areas_state_representation_server_ = this->public_node_handle_.advertiseService("get_dirty_areas_state_representation", &CleaningPlanAlgNode::get_dirty_areas_state_representationCallback, this);
        this->get_trajectories_from_plan_server_ = this->public_node_handle_.advertiseService("get_trajectories_from_plan", &CleaningPlanAlgNode::get_trajectories_from_planCallback, this);
        
        // [init clients]
        
        // [init action servers]
        
        // [init action clients]
        
        // Other
        
}

CleaningPlanAlgNode::~CleaningPlanAlgNode(void)
{
        // [free dynamic memory]
}

void CleaningPlanAlgNode::mainNodeThread(void)
{
        // [fill msg structures]
        
        // [fill srv structure and make request to the server]
        
        // [fill action structure and make request to the action server]

        // [publish messages]
        
}

/*  [subscriber callbacks] */

/*  [service callbacks] */
bool CleaningPlanAlgNode::representation_to_stringCallback(estirabot_msgs::RepresentationToString::Request &req, estirabot_msgs::RepresentationToString::Response &res) 
{ 
        ROS_DEBUG("CleaningPlanAlgNode::representation_to_stringCallback: New Request Received!"); 

        //use appropiate mutex to shared variables if necessary 
        //this->alg_.lock(); 
        //this->representation_to_string_mutex_.enter(); 

    // TODO remove traversed_ellipses from request
        res.state_string = this->ellipses_representation.getStateString(req.dirty_areas);
        
        //unlock previously blocked shared variables 
        //this->alg_.unlock(); 
        //this->representation_to_string_mutex_.exit(); 

        return true; 
}

bool CleaningPlanAlgNode::get_dirty_areas_state_representationCallback(iri_clean_board::StateRepresentation::Request &req, iri_clean_board::StateRepresentation::Response &res) 
{ 
        ROS_INFO("CleaningPlanAlgNode::get_dirty_areas_state_representationCallback: New Request Received!"); 
        
        cv_bridge::CvImagePtr cv_ptr;
        
        try
        {
                cv_ptr = cv_bridge::toCvCopy(req.image, sensor_msgs::image_encodings::TYPE_8UC1);
        }
        catch (cv_bridge::Exception& e)
        {
                ROS_ERROR("cv_bridge exception: %s", e.what());
                return false;
        }
        
        //use appropiate mutex to shared variables if necessary 
        this->alg_.lock(); 
        this->get_dirty_areas_state_representation_mutex_.enter(); 
        
        this->ellipses_representation.createNewState(cv_ptr->image);
        
        for( size_t i = 0; i < ellipses_representation.getEllipsesNumber(); i++ ) { // Debugging info
                ROS_DEBUG_STREAM("ELIPSES: DATA");
                ROS_DEBUG_STREAM("Center: " << ellipses_representation.getCurrentEllipses()[i].center << 
                                 " Size: " << ellipses_representation.getCurrentEllipses()[i].size.width << "," << ellipses_representation.getCurrentEllipses()[i].size.height);
                ROS_DEBUG_STREAM("Angle: " << (ellipses_representation.getCurrentEllipses()[i].angle/180)*M_PI);
        }
        
        if (ellipses_representation.getEllipsesNumber() > 0) { // if there's something to clean
                res.dirty_areas  = this->ellipses_representation.getRosDirtyAreas();
                res.distances    = this->ellipses_representation.distancesBetweenEllipses(res.dirty_areas);
                res.traversed_ellipses = this->ellipses_representation.getTraversedEllipses(res.dirty_areas);
                res.state_string = this->ellipses_representation.getStateString(res.dirty_areas);
                res.arm_position = this->ellipses_representation.getNearestEllipse();
                res.is_clean     = false;
        }
        else {
                res.state_string = "";
                res.is_clean = true; // nothing to clean
        }
        
        this->ellipses_representation.writeStateToFile(this->file_planner_state, res.dirty_areas);
        
        //unlock previously blocked shared variables 
        this->alg_.unlock(); 
        this->get_dirty_areas_state_representation_mutex_.exit(); 

        return true; 
}


bool CleaningPlanAlgNode::get_trajectories_from_planCallback(iri_clean_board::PlanToTrajectories::Request &req, iri_clean_board::PlanToTrajectories::Response &res) 
{ 
        ROS_DEBUG("CleaningPlanAlgNode::get_trajectories_from_planCallback: New Request Received!"); 
        
        //use appropiate mutex to shared variables if necessary 
        //this->alg_.lock(); 
        //this->get_trajectories_from_plan_mutex_.enter(); 
        
        savePlanningResponse(req, res);
        
        if ( (res.poses.size()>0) && (this->image_plan_out_publisher_.getNumSubscribers()>0) ){ // Publish plan image
                cv_bridge::CvImage cvbridge_aux;
                cv::Mat image_bgr;
                cvbridge_aux.encoding     = sensor_msgs::image_encodings::BGR8;
                cvbridge_aux.image        = this->ellipses_representation.show_image;
                cvbridge_aux.header.stamp = ros::Time::now();
                
                this->Image_msg_ = *cvbridge_aux.toImageMsg();
                this->image_plan_out_publisher_.publish(this->Image_msg_);
        }
        
        //unlock previously blocked shared variables 
        //this->alg_.unlock(); 
        //this->get_trajectories_from_plan_mutex_.exit(); 
        
        return true; 
}


void CleaningPlanAlgNode::savePlanningResponse(iri_clean_board::PlanToTrajectories::Request req, 
                                                 iri_clean_board::PlanToTrajectories::Response &res)
{
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
        bool is_real_movement;
        
        size_t i = 0; // max_actions = this->max_number_actions || max_actions = num_actions
        uint j = 0; // max movements = #ellipses  // movements don't include move(X)
        
        if (this->no_planning) {
                getNearestCleanPlan(req, res);
                return;
        }
        else if (this->no_perception) {
                getNoPerceptionCleanPlan(req, res);
                return;
        }
        
        pcl::fromROSMsg (req.pointcloud_in, *cloud);
    // debugging info
        show_dirty_areas_information(req.pointcloud_in, req.dirty_areas);
        
        ROS_INFO_STREAM("Got number of actions: " << req.actions.size());
        
        while ( (i < req.actions.size()) && 
                (j < this->max_number_actions) && (j < req.dirty_areas.size() ) ) 
        {
                // print executed action
                std::stringstream ss;
                for (size_t k = 0; k< req.target_ellipses[i].indexes.size(); ++k) { ss << req.target_ellipses[i].indexes[k] << ","; }
                ROS_INFO_STREAM("Adding movement " << req.actions[i] << "(" << ss.str() << ")");
                
                is_real_movement= this->alg_.getMovementFromAction(req.dirty_areas, req.actions[i], 
                                                                   req.target_ellipses[i],
                                                                   this->ellipses_representation,
                                                                   cloud, req.plane_coefficients, 
                                                                   res.poses, res.poses_2d_coordinates, res.secondary_arm );
                if (is_real_movement)
                        j++;
                i++;
        }
        
        if (j == 0) { // something to clean but planner didn't return any movement
                res.movement = false;
        }
        else { // something to clean and planner returned a plan
                res.movement = true;
        }
}


void CleaningPlanAlgNode::show_dirty_areas_information(sensor_msgs::PointCloud2 pointcloud, std::vector< estirabot_msgs::DirtyArea > dirty_areas)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    visualization_msgs::MarkerArray markers_msg;
    
    if(this->visualization_markers_publisher_.getNumSubscribers())
    {
        pcl::fromROSMsg (pointcloud, *pcl_cloud);
        
        for (size_t i = 0; i < dirty_areas.size(); i++) {
                // marker
                visualization_msgs::Marker marker;
                marker.header.frame_id = pointcloud.header.frame_id; //"/camera_rgb_optical_frame";
                marker.ns = "Dirty areas";
                marker.id = i;
                marker.header.stamp = ros::Time();
                marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
                marker.action = visualization_msgs::Marker::ADD;
                
                pcl::PointXYZ point = pcl_cloud->at(dirty_areas[i].ellipse.center.x,dirty_areas[i].ellipse.center.y);
                marker.pose.position.x = point.x;
                marker.pose.position.y = point.y;
                marker.pose.position.z = point.z - 0.05;
                marker.pose.orientation.x = 0.0;
                marker.pose.orientation.y = 0.0;
                marker.pose.orientation.z = 0.0;
                marker.pose.orientation.w = 1.0;
                
                // scale
                marker.scale.x = 0.1;
                marker.scale.y = 0.1;
                marker.scale.z = 0.1;
                // visible
                marker.color.a = 1.0;
                marker.color.r = 1.0;
                marker.color.g = 0.0;
                marker.color.b = 0.0;
                
                std::stringstream ss;
                ss << 60 + dirty_areas[i].id;
                marker.text=ss.str();
                
                // add to array
                markers_msg.markers.push_back(marker);
        }
        
        // remove posible remaining markers
        for (size_t i = markers_msg.markers.size(); i < 10; i++) {
                // marker
                visualization_msgs::Marker marker;
                marker.header.frame_id = pointcloud.header.frame_id; //"/camera_rgb_optical_frame";
                marker.ns = "Dirty areas";
                marker.id = i;
                marker.header.stamp = ros::Time();
                marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
                marker.action = visualization_msgs::Marker::ADD;
                
                marker.pose.position.x = 0.0;
                marker.pose.position.y = 0.0;
                marker.pose.position.z = (0.1 * i);
                marker.pose.orientation.x = 0.0;
                marker.pose.orientation.y = 0.0;
                marker.pose.orientation.z = 0.0;
                marker.pose.orientation.w = 1.0;
                
                // scale
                marker.scale.x = 0.1;
                marker.scale.y = 0.1;
                marker.scale.z = 0.1;
                // visible
                marker.color.a = 0.0;
                marker.color.r = 1.0;
                marker.color.g = 0.0;
                marker.color.b = 0.0;
                
                marker.text="as";
                
                markers_msg.markers.push_back(marker);
        }
        
        this->MarkerArray_msg_ = markers_msg;
    
    
        this->visualization_markers_publisher_.publish(this->MarkerArray_msg_);
    }
}


void CleaningPlanAlgNode::getNearestCleanPlan(iri_clean_board::PlanToTrajectories::Request req, 
                                                iri_clean_board::PlanToTrajectories::Response &res)
{
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
        
        pcl::fromROSMsg (req.pointcloud_in, *cloud);
        
//      std::vector< size_t > plan = ellipses_representation.getGoodPathThroughAllEllipses(ellipses_representation.getNearestEllipse());
    std::vector< size_t > plan = ellipses_representation.getGoodPathThroughAllDirtyAreas(req.dirty_areas);
        std::vector< size_t >::iterator plan_it;

        for ( plan_it = plan.begin() ; plan_it < plan.end(); plan_it++ ) {
                estirabot_msgs::ArrayIndexes target_ellipses;
                std::string movement_type;
                target_ellipses.indexes.push_back(*plan_it);
                
                if (this->board_plan) {
                        cv::RotatedRect ellipse = this->ellipses_representation.getCurrentEllipses()[*plan_it];
                        
                        movement_type = "smallClean";
                        if (1) { // use actions also
                                if (ellipse.size.width > 50)
                                        movement_type = "circularClean";
                                else if (ellipse.size.height > 50)
                                        movement_type = "straightClean";
                        }
                }
                else
                        movement_type = "pickUp";
                
                alg_.getMovementFromAction(req.dirty_areas, "pickUp", target_ellipses, 
                                        this->ellipses_representation,
                                        cloud, req.plane_coefficients, 
                                        res.poses, res.poses_2d_coordinates, res.secondary_arm );
        }
        
        res.movement = true;
}

void CleaningPlanAlgNode::getNoPerceptionCleanPlan(iri_clean_board::PlanToTrajectories::Request req, iri_clean_board::PlanToTrajectories::Response &res)
{
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
        pcl::fromROSMsg (req.pointcloud_in, *cloud);
        
        ROS_INFO_STREAM("Creating trajectories without ellipses perception.");
        req.actions.clear();
        req.actions.push_back("NoPerception");
        this->alg_.getMovementFromAction(req.dirty_areas, req.actions[0], 
                                                                   req.target_ellipses[0],
                                                                   this->ellipses_representation,
                                                                   cloud, req.plane_coefficients, 
                                                                   res.poses, res.poses_2d_coordinates, res.secondary_arm );
        
        res.movement = true;
}


/*  [action callbacks] */

/*  [action requests] */

void CleaningPlanAlgNode::node_config_update(Config &config, uint32_t level)
{
        this->alg_.lock();
        
        this->max_number_actions = config.max_number_actions;
        this->no_planning = config.no_planning;
        this->no_perception = config.no_perceptions;
        this->board_plan = config.board_plan;
        this->file_planner_state = config.file_planner_state;
        
        // update ellipses_representation config
        this->ellipses_representation.config_update(config, level);
        
        this->alg_.unlock();
}

void CleaningPlanAlgNode::addNodeDiagnostics(void)
{
}

/* main function */
int main(int argc,char *argv[])
{
        return algorithm_base::main<CleaningPlanAlgNode>(argc, argv, "cleaning_plan_alg_node");
}