moped_actionclient_alg_node.cpp

Go to the documentation of this file.

#include "moped_actionclient_alg_node.h"

MopedActionclientAlgNode::MopedActionclientAlgNode(void) :
        algorithm_base::IriBaseAlgorithm<MopedActionclientAlgorithm>(),
        moped_client_("moped", true)
{
        //init class attributes if necessary
        //this->loop_rate_ = 2;//in [Hz]

        // [init publishers]
        
        // [init subscribers]
        
        // [init services]
        
        // [init clients]
        
        // [init action servers]
        
        // [init action clients]
}

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

void MopedActionclientAlgNode::mainNodeThread(void)
{
        this->image = cv::imread("/home/frigual/ros/stacks/moped/moped-example/test/query-close.jpg", 0 );

        // [fill msg structures]
        
        // [fill srv structure and make request to the server]
        
        // [fill action structure and make request to the action server]
        mopedMakeActionRequest();

        // [publish messages]
}

/*      [subscriber callbacks] */

/*      [service callbacks] */

/*      [action callbacks] */
void MopedActionclientAlgNode::mopedDone(const actionlib::SimpleClientGoalState& state, const iri_moped_actionserver::mopedResultConstPtr& result) 
{ 
        if( state.toString().compare("SUCCEEDED") == 0 ) 
                ROS_INFO("MopedActionclientAlgNode::mopedDone: Goal Achieved!"); 
        else 
                ROS_INFO("MopedActionclientAlgNode::mopedDone: %s", state.toString().c_str()); 

        //copy & work with requested result 
} 

void MopedActionclientAlgNode::mopedActive() 
{ 
        //ROS_INFO("MopedActionclientAlgNode::mopedActive: Goal just went active!"); 
} 

void MopedActionclientAlgNode::mopedFeedback(const iri_moped_actionserver::mopedFeedbackConstPtr& feedback) 
{ 
        //ROS_INFO("MopedActionclientAlgNode::mopedFeedback: Got Feedback!"); 

        bool feedback_is_ok = true; 

        //analyze feedback 
        //my_var = feedback->var; 

        //if feedback is not what expected, cancel requested goal 
        if( !feedback_is_ok ) 
        { 
                moped_client_.cancelGoal(); 
                //ROS_INFO("MopedActionclientAlgNode::mopedFeedback: Cancelling Action!"); 
        } 
}

/*      [action requests] */
void MopedActionclientAlgNode::mopedMakeActionRequest() 
{ 
        ROS_INFO("MopedActionclientAlgNode::mopedMakeActionRequest: Starting New Request!"); 

        //wait for the action server to start 
        //will wait for infinite time 
        moped_client_.waitForServer();  
        ROS_INFO("MopedActionclientAlgNode::mopedMakeActionRequest: Server is Available!"); 

        // Check if image is loaded
        if( this->image.empty() )
        {
                ROS_ERROR("Trying to send image but it's not loaded.");
                return;
        }
        else
        {
                cv::namedWindow("title", CV_WINDOW_NORMAL);
                cv::imshow("title",this->image);
                cv::waitKey(0);
                cv::destroyWindow("title");
        }

        //send a goal to the action 
        cv_bridge::CvImagePtr cv_ptr(new cv_bridge::CvImage);
        cv_ptr->image = this->image;
        cv_ptr->encoding = "mono8";

        moped_goal_.image = *(cv_ptr->toImageMsg());

        moped_client_.sendGoal(moped_goal_, 
                                                        boost::bind(&MopedActionclientAlgNode::mopedDone,                this, _1, _2), 
                                                        boost::bind(&MopedActionclientAlgNode::mopedActive,      this), 
                                                        boost::bind(&MopedActionclientAlgNode::mopedFeedback, this, _1)); 

        ROS_INFO("MopedActionclientAlgNode::mopedMakeActionRequest: Goal Sent. Wait for Result!"); 
}

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

        this->alg_.unlock();
}

void MopedActionclientAlgNode::addNodeDiagnostics(void)
{
}

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