object_pose_detection_alg_node.cpp

Go to the documentation of this file.

#include "object_pose_detection_alg_node.h"

ObjectPoseDetectionAlgNode::ObjectPoseDetectionAlgNode(void) :
        algorithm_base::IriBaseAlgorithm<ObjectPoseDetectionAlgorithm>(),
        obj_pose_det_aserver_(public_node_handle_, "obj_pose_det")
{
        //init class attributes if necessary
        //this->loop_rate_ = 2;//in [Hz]

        // [init publishers]
        
        // [init subscribers]
        
        // [init services]
        
        // [init clients]
        
        // [init action servers]
        obj_pose_det_aserver_.registerStartCallback(boost::bind(&ObjectPoseDetectionAlgNode::obj_pose_detStartCallback, this, _1)); 
        obj_pose_det_aserver_.registerStopCallback(boost::bind(&ObjectPoseDetectionAlgNode::obj_pose_detStopCallback, this)); 
        obj_pose_det_aserver_.registerIsFinishedCallback(boost::bind(&ObjectPoseDetectionAlgNode::obj_pose_detIsFinishedCallback, this)); 
        obj_pose_det_aserver_.registerHasSucceedCallback(boost::bind(&ObjectPoseDetectionAlgNode::obj_pose_detHasSucceedCallback, this)); 
        obj_pose_det_aserver_.registerGetResultCallback(boost::bind(&ObjectPoseDetectionAlgNode::obj_pose_detGetResultCallback, this, _1)); 
        obj_pose_det_aserver_.registerGetFeedbackCallback(boost::bind(&ObjectPoseDetectionAlgNode::obj_pose_detGetFeedbackCallback, this, _1)); 
        obj_pose_det_aserver_.start();
        
        // [init action clients]
}

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

void ObjectPoseDetectionAlgNode::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]

        //alg_.lock();
        //ok_ = alg_.detect_object(goal.image, goal.pcl);
        //alg_.unlock();
}

/*      [subscriber callbacks] */

/*      [service callbacks] */

/*      [action callbacks] */
void ObjectPoseDetectionAlgNode::obj_pose_detStartCallback(const iri_perception_msgs::object_pose_detectionGoalConstPtr& goal)
{ 
        alg_.lock();
        // Joan Perez comments:
        //   check goal (Image + PCL2)
        //   image_=goal.image;
        //   execute goal

        // TODO: Check goal's correctness.

        this->queryImage = goal.???; //TODO: correct.
        alg_.unlock();
} 

void ObjectPoseDetectionAlgNode::obj_pose_detStopCallback(void) 
{ 
        alg_.lock(); 
                //stop action 
        alg_.unlock(); 
} 

bool ObjectPoseDetectionAlgNode::obj_pose_detIsFinishedCallback(void) 
{ 
        bool ret = false; 

        alg_.lock(); 
                //if action has finish for any reason 
                //ret = ok_; 
        alg_.unlock(); 

        return ret; 
} 

bool ObjectPoseDetectionAlgNode::obj_pose_detHasSucceedCallback(void) 
{ 
        bool ret = false; 

        alg_.lock(); 
                //if goal was accomplished 
                //      ret = true 
        alg_.unlock(); 

        return ret; 
} 

void ObjectPoseDetectionAlgNode::obj_pose_detGetResultCallback(iri_perception_msgs::object_pose_detectionResultPtr& result) 
{ 
        alg_.lock(); 
                //update result data to be sent to client 
                //result->pose = data; 
        alg_.unlock(); 
} 

void ObjectPoseDetectionAlgNode::obj_pose_detGetFeedbackCallback(iri_perception_msgs::object_pose_detectionFeedbackPtr& feedback) 
{ 
        alg_.lock(); 
                //keep track of feedback 
                //ROS_INFO("feedback: %s", feedback->data.c_str()); 
        alg_.unlock(); 
}

/*      [action requests] */

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

        this->alg_.unlock();
}

void ObjectPoseDetectionAlgNode::addNodeDiagnostics(void)
{
}

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