get_board_information_alg_node.cpp

Go to the documentation of this file.

#include "get_board_information_alg_node.h"


GetBoardInformationAlgNode::GetBoardInformationAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<GetBoardInformationAlgorithm>(),
  EventInfoReady("Info ready"),
  EventServiceWaiting("Service Waiting")
{
        //init class attributes if necessary
        //this->loop_rate_ = 2;//in [Hz]
        
        // [init publishers]
  this->believe_status_publisher_ = this->public_node_handle_.advertise<sensor_msgs::Image>("believe_status", 1);
        this->pointcloud_out_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>("pointcloud_out", 1);
        
        // [init subscribers]
        this->unreachable_position_subscriber_ = this->public_node_handle_.subscribe("unreachable_position", 10, &GetBoardInformationAlgNode::unreachable_position_callback, this);
        this->plane_coefficients_subscriber_ = this->public_node_handle_.subscribe("plane_coefficients", 1, &GetBoardInformationAlgNode::plane_coefficients_callback, this);
        this->pointcloud_in_subscriber_ = this->public_node_handle_.subscribe("pointcloud_in", 0, &GetBoardInformationAlgNode::pointcloud_in_callback, this);
        this->image_in_subscriber_ = this->public_node_handle_.subscribe("image_in", 1, &GetBoardInformationAlgNode::image_in_callback, this);
        
        // [init services]
        this->get_board_info_server_ = this->public_node_handle_.advertiseService("get_board_info_srv", &GetBoardInformationAlgNode::get_board_infoCallback, this);
        
        // [init clients]
        
        // [init action servers]
        
        // [init action clients]
        
        // other
        is_service_waiting = false;
        waiting_for_pointcloud = false;
        waiting_for_image = false;
        waiting_for_coefficients = false;
        
        // initial non-existing image
        cv_bridge::CvImage cvbridge_aux;
        cvbridge_aux.encoding = sensor_msgs::image_encodings::BGR8;
        cvbridge_aux.image    = cv::Mat::zeros(cv::Size(640,480), CV_8UC3);
        cvbridge_aux.header.stamp = ros::Time::now();
        last_image = *cvbridge_aux.toImageMsg();
}

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

void GetBoardInformationAlgNode::mainNodeThread(void)
{
        // [fill msg structures]
  //this->Image_msg_.data = my_var;
        //this->PointCloud2_msg_.data = my_var;
        
        // [fill srv structure and make request to the server]
        
        // [fill action structure and make request to the action server]

        // [publish messages]
        //this->believe_status_publisher_.publish(this->Image_msg_);
        //this->pointcloud_out_publisher_.publish(this->PointCloud2_msg_);
        
        waiting_for_pointcloud = true;
        waiting_for_image = true;
        waiting_for_coefficients = true;
        
        // wait for the event to be signales. This function will get blocked until
        // the timeout expires.
        while (waiting_for_image || waiting_for_coefficients) {
                try {
                        EventInfoReady.wait(5000000); // 5 secs
                }catch(CEventTimeoutException& e){
                        //std::cout << e.what() << std::endl;
                        ROS_ERROR_STREAM("get_board_info -> main_thread: timed out!");
                        ROS_INFO_STREAM("Got pointcloud: " << not waiting_for_pointcloud);
                        ROS_INFO_STREAM("Got image:" << not waiting_for_image);
                        ROS_INFO_STREAM("Got coefficients: " << not waiting_for_coefficients);
                        
                        // retry -> reset booleans
                        waiting_for_pointcloud = true;
                        waiting_for_image = true;
                        waiting_for_coefficients = true;
                }catch(CException& e){
                        //std::cout << e.what() << std::endl;
                        ROS_ERROR("get_board_info -> main_thread: Exception found waiting");
                        return;
                }
        }
        
        this->alg_.lock();
        last_pointcloud = current_pointcloud; 
        //res.segmented_image = current_image; 
        last_image = current_image;
        last_plane_coeffs = current_plane_coeffs;
        this->alg_.unlock();
        
        // publish current believe
        this->Image_msg_ = this->alg_.getCurrentBelieveColors();
        
        if(this->believe_status_publisher_.getNumSubscribers())
                this->believe_status_publisher_.publish(this->Image_msg_);
        
        if (is_service_waiting) {
                is_service_waiting = false;
                EventServiceWaiting.set();
        }
}

/*  [subscriber callbacks] */
void GetBoardInformationAlgNode::unreachable_position_callback(const estirabot_msgs::ImagePoint::ConstPtr& msg) 
{ 
        ROS_DEBUG("GetBoardInformationAlgNode::unreachable_position_callback: New Message Received"); 

        //use appropiate mutex to shared variables if necessary 
        //this->alg_.lock(); 
        //this->unreachable_position_mutex_.enter(); 
        
        //std::cout << msg->data << std::endl; 
        
        alg_.updateUnreachableZones(msg);
        
        //unlock previously blocked shared variables 
        //this->alg_.unlock(); 
        //this->unreachable_position_mutex_.exit(); 
}
void GetBoardInformationAlgNode::plane_coefficients_callback(const pcl_msgs::ModelCoefficients::ConstPtr& msg) 
{ 
        ROS_DEBUG("GetBoardInformationAlgNode::plane_coefficients_callback: New Message Received"); 

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

        //std::cout << msg->data << std::endl; 
        current_plane_coeffs = *msg;
        waiting_for_coefficients = false;
        EventInfoReady.set();

        //unlock previously blocked shared variables 
        //this->alg_.unlock(); 
        //this->plane_coefficients_mutex_.exit(); 
}
void GetBoardInformationAlgNode::pointcloud_in_callback(const sensor_msgs::PointCloud2::ConstPtr& msg) 
{ 
        //ROS_INFO("GetBoardInformationAlgNode::pointcloud_in_callback: New Message Received"); 

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

        //std::cout << msg->data << std::endl; 
        //if (!has_pointcloud)
        //      ROS_INFO("GetBoardInformationAlgNode::pointcloud_in_callback: Pointcloud available"); 
        
        // if it was waiting for a pointcloud to be ready, publish it
        if (waiting_for_pointcloud) {
                ROS_DEBUG("GetBoardInformationAlgNode::pointcloud_in_callback: Pointcloud available");
                current_pointcloud = *msg;
                //this->PointCloud2_msg_ = *msg;
                this->pointcloud_out_publisher_.publish(*msg);
                waiting_for_pointcloud = false;
        }

        //unlock previously blocked shared variables 
        //this->alg_.unlock(); 
        //this->pointcloud_in_mutex_.exit(); 
}
void GetBoardInformationAlgNode::image_in_callback(const sensor_msgs::Image::ConstPtr& msg) 
{ 
        ROS_DEBUG("GetBoardInformationAlgNode::image_in_callback: New Message Received"); 

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

        //std::cout << msg->data << std::endl; 
        current_image = *msg;
        this->alg_.updateCurrentBelieve(msg);
        waiting_for_image = false;

        //unlock previously blocked shared variables 
        //this->alg_.unlock(); 
        EventInfoReady.set();
        //this->image_in_mutex_.exit(); 
}

/*  [service callbacks] */
bool GetBoardInformationAlgNode::get_board_infoCallback(iri_clean_board::BoardInfo::Request &req, iri_clean_board::BoardInfo::Response &res) 
{ 
        ROS_DEBUG("GetBoardInformationAlgNode::get_board_infoCallback: New Request Received!"); 
        
        this->get_board_info_mutex_.enter(); 
        
        // wait for new image, for having updated data
        
        /*is_service_waiting = true;
        while (is_service_waiting) {
                try {
                        EventServiceWaiting.wait(1000000);
                }catch(CEventTimeoutException& e){
                        std::cout << e.what() << std::endl;
                }catch(CException& e){
                        std::cout << e.what() << std::endl;
                        return false;
                }
        }*/
        // fill service with new data
        this->alg_.lock();
                res.pointcloud = last_pointcloud; 
                res.segmented_image = this->alg_.joinImageAndBelieve(last_image);
                res.plane_coefficients = last_plane_coeffs.values;
        this->alg_.unlock();
        
        this->get_board_info_mutex_.exit();
        
        return true;
}

/*  [action callbacks] */

/*  [action requests] */


void GetBoardInformationAlgNode::node_config_update(Config &config, uint32_t level)
{
        this->alg_.lock();

        this->alg_.unlock();
}

void GetBoardInformationAlgNode::addNodeDiagnostics(void)
{
}

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