iri_deformable_analysis.cpp

Go to the documentation of this file.

#include "iri_deformable_analysis.h"
#include "iri_deformable_analysis/SetScoreMixAlgorithmID.h"
#include <sensor_msgs/PointCloud2.h>

IRIDeformableAnalysis::IRIDeformableAnalysis() :
    score_results_recieved_event_("score_results")
{
    /* Enable the input service */
    deformable_service_ = nh_.advertiseService("do_deformable_analysis", 
                                               & IRIDeformableAnalysis::deformable_analysis_callback, this);
    /* Output topic */
    deformable_pcl_publisher_ = nh_.advertise<sensor_msgs::PointCloud2>("pcl_input", 100);
    /* Returning topic */
    score_results_subscriber_ = nh_.subscribe<iri_deformable_analysis::ScoreMapMixResults>("deformable_score_results", 1,
                                                          & IRIDeformableAnalysis::score_results_callback, this);
    score_results_recieved_event_.reset();
    ROS_INFO("Deformable analysis ready");
}

bool
IRIDeformableAnalysis::deformable_analysis_callback(iri_deformable_analysis::DoDeformableAnalysis::Request & req,
                                                    iri_deformable_analysis::DoDeformableAnalysis::Response & res)
{
    ROS_INFO("Deformable analysis request recieved");

    /* 1. Call to config */
    iri_deformable_analysis::SetScoreMixAlgorithmID algorihtm_srv;
    algorihtm_srv.request.algorithm_id = req.fusion_criteria_id;

    if (! ros::service::exists("score_map_mix_node/set_score_mix_algorithm_id", true)) {
        ROS_ERROR("Algorithm ID can not be set");
        return false;
    }

    if (! ros::service::call("score_map_mix_node/set_score_mix_algorithm_id", algorihtm_srv)) {
        ROS_WARN("Set fusion algorithm criteria failed");
        return false;
    }

    /* 2. Publish PCL */
    deformable_pcl_publisher_.publish(req.pcl_to_analyze);

    /* 3. Wait for response */
    try
    {
        score_results_recieved_event_.wait(timeout_for_score_results_);
        ROS_INFO("Analysis recieved");
        score_results_recieved_event_.reset();
    }
    catch (CEventTimeoutException & e)
    {
        ROS_WARN("Timeout for deformable analysis");
        score_results_recieved_event_.reset();
        return false;
    }
    catch (CEventException & e)
    {
        ROS_WARN("Unexpected event exception %s", e.what().c_str());
        score_results_recieved_event_.reset();
        return false;
    }

    /* 4. return the service response */
    res.deformable_analysis.graspability.best_grasp_pose = last_score_results_.best_scored_pose;

    return true;
}

void
IRIDeformableAnalysis::score_results_callback(const iri_deformable_analysis::ScoreMapMixResults::ConstPtr & msg)
{
    /* Save the results and activate the event */
    ROS_INFO("Score results recieved");
    last_score_results_ = * msg;
    score_results_recieved_event_.set();
}

// Set up a default time of 60 seconds
const IRIDeformableAnalysis::microseconds IRIDeformableAnalysis::timeout_for_score_results_ = 60 * 1000 * 1000;

int main(int argc, char** argv)
{
    ros::init(argc, argv, "deformable_analysis");
    IRIDeformableAnalysis server;

    ros::AsyncSpinner spinner(4); // Use 4 threads
    spinner.start();
    ros::waitForShutdown();

    return 0;
}