qnode_tibi_coop.cpp

Go to the documentation of this file.

/*****************************************************************************
** Includes
*****************************************************************************/

#include <ros/ros.h>
#include <ros/network.h>
#include <string>
#include <std_msgs/String.h>
#include <sstream>
#include "../include/tibi_dabo_gui/qnode_tibi_coop.hpp"

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace tibi_dabo_gui {

/*****************************************************************************
** Implementation
*****************************************************************************/

QNode::QNode(int argc, char** argv ) :
  init_argc(argc),
  init_argv(argv),
  gui_action("gui",true)
  {}

QNode::~QNode() {
  if(ros::isStarted()) {
    ros::shutdown(); // explicitly needed since we use ros::start();
    ros::waitForShutdown();
  }
  wait();
}

bool QNode::initialize()
{
  if ( ! ros::master::check() ) {
      return false;
  }
  ros::start(); // explicitly needed since our nodehandle is going out of scope.
  ros::NodeHandle n;

  // [service clients]
  get_locations_client = n.serviceClient<iri_goal_database::get_locations>("get_locations");

  //[action clients]
      // this->gui_goal.FIELD=X
      // guiMakeActionRequest();
  this->gui_active=false;

  start();
  return true;
}

void QNode::run() {
  ros::Rate loop_rate(100);
  int count = 0;
  while ( ros::ok() )
  {
    ros::spinOnce();
    loop_rate.sleep();
    ++count;
  }
  std::cout << "Ros shutdown, proceeding to close the gui." << std::endl;
  Q_EMIT rosShutdown(); // used to signal the gui for a shutdown (useful to roslaunch)
}

void QNode::askGoal(std::string goal)
{
  mutex.lock();
  if(!this->gui_active)
  {
    this->gui_active=true;
    this->gui_goal.cmd_id=0;
    this->gui_goal.string_params.resize(1);
    this->gui_goal.string_params[0]=goal;
    if(!guiMakeActionRequest())
      ROS_ERROR("QNode::askGoal: guiMakeActionRequest failed!");
  }
  else
    ROS_INFO("QNode::askGoal: guiAction active, goal not sent!");
  mutex.unlock();
}

bool QNode::getLocations(std::vector<std::string> & locations)
{
  mutex.lock();
  //get locations from database service, fill locations vector
  bool ok=true;
  if(get_locations_client.call(this->get_locations_srv))
  {
    for(unsigned int i=0; i<this->get_locations_srv.response.locations.size(); i++)
    {
      locations.push_back(this->get_locations_srv.response.locations[i]);
    }
  }
  else
  {
    ok=false;
    ROS_ERROR("QNode::getLocations: unable to call get_locations service!");
  }
  mutex.unlock();
  return ok;
}

bool QNode::guiActionActive()
{
  return this->gui_active;
}

//gui action client functions
void QNode::guiDone(const actionlib::SimpleClientGoalState& state,  const tibi_dabo_msgs::guiResultConstPtr& result)
{
  mutex.lock();
  if(state.state_==actionlib::SimpleClientGoalState::SUCCEEDED)
    ROS_INFO("QNode::guiDone: Goal Achieved!");
  else if(state.state_==actionlib::SimpleClientGoalState::ABORTED)
    ROS_INFO("QNode::guiDone: Goal aborted!");
  else if(state.state_==actionlib::SimpleClientGoalState::PREEMPTED)
    ROS_INFO("QNode::guiDone: Goal preempted!");
  else
    ROS_INFO("QNode::guiDone: Other termination state: PENDING, ACTIVE, RECALLED, REJECTED, LOST");
  this->gui_active=false;
  mutex.unlock();
}

void QNode::guiActive()
{
  mutex.lock();
  ROS_INFO("QNode::guiActive: Goal just went active!"); 
  this->gui_active=true;
  mutex.unlock();
}

void QNode::guiFeedback(const tibi_dabo_msgs::guiFeedbackConstPtr& feedback)
{
  mutex.lock();
  //ROS_INFO("QNode::guiFeedback: feedback received!");
  bool feedback_is_ok = true;
  if( !feedback_is_ok )
    gui_action.cancelGoal();
  mutex.unlock();
}

bool QNode::guiMakeActionRequest() 
{
   ROS_INFO("QNode::guiMakeActionRequest: Starting New Request!"); 
   gui_action.waitForServer();
   ROS_INFO("QNode::guiMakeActionRequest: Server is available!"); 
   gui_action.sendGoal(this->gui_goal, 
               boost::bind(&QNode::guiDone,     this, _1, _2), 
               boost::bind(&QNode::guiActive,   this), 
               boost::bind(&QNode::guiFeedback, this, _1)); 
   ROS_INFO("QNode::guiMakeActionRequest: Goal Sent. Wait for Result!"); 
   return true;
}

}  // namespace tibi_dabo_gui