qnode_car_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_car_coop.hpp"

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

namespace tibi_dabo_gui {

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

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

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_active=false;

  // [action servers]
  gui_action_aserver_.registerStartCallback(boost::bind(&QNode::gui_actionStartCallback, this, _1)); 
  gui_action_aserver_.registerStopCallback(boost::bind(&QNode::gui_actionStopCallback, this)); 
  gui_action_aserver_.registerIsFinishedCallback(boost::bind(&QNode::gui_actionIsFinishedCallback, this)); 
  gui_action_aserver_.registerHasSucceedCallback(boost::bind(&QNode::gui_actionHasSucceedCallback, this)); 
  gui_action_aserver_.registerGetResultCallback(boost::bind(&QNode::gui_actionGetResultCallback, this, _1)); 
  gui_action_aserver_.registerGetFeedbackCallback(boost::bind(&QNode::gui_actionGetFeedbackCallback, this, _1)); 
  gui_action_aserver_.start();
  this->new_gui_cmd=false;
  this->gui_cmd=gui_none;

  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::go()
{
  mutex.lock();
  if(!this->gui_active)
  {
    this->gui_active=true;
    this->gui_goal.cmd_id=0;
    if(!guiMakeActionRequest())
      ROS_ERROR("QNode::go: guiMakeActionRequest failed!");
  }
  else
    ROS_INFO("QNode::go: 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::getGoal(std::string & goal)
{
  mutex.lock();
  bool ok=true;
  if(this->new_gui_cmd)
  {
    this->new_gui_cmd=false;
    goal=this->gui_location_id;
  }
  else
    ok=false;
  mutex.unlock();
  return ok;
}

//gui action server
void QNode::gui_actionStartCallback(const tibi_dabo_msgs::guiGoalConstPtr& goal)
{
  ROS_INFO("QNode::gui_actionStartCallback"); 
  mutex.lock(); 
    //check goal 
  this->gui_cmd=(gui_cmds)goal->cmd_id;
  switch(this->gui_cmd)
  {
    case gui_set_goal: 
      if(goal->string_params.size()==1)
      {
        ROS_INFO("QNode::gui_actionStartCallback: goal_id=%s",goal->string_params[0].c_str());
        this->gui_location_id=goal->string_params[0];
        this->new_gui_cmd=true;
      }
      else
        this->new_gui_cmd=false;
      break;
    default: this->new_gui_cmd=false; 
    break;
  }
  mutex.unlock(); 
} 

void QNode::gui_actionStopCallback(void) 
{ 
  mutex.lock(); 
    //stop action 
  // cancel the goal
  mutex.unlock(); 
} 

bool QNode::gui_actionIsFinishedCallback(void) 
{ 
  bool ret = false; 

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

  return ret; 
} 

bool QNode::gui_actionHasSucceedCallback(void) 
{ 
  bool ret = false; 

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

  return ret; 
} 

void QNode::gui_actionGetResultCallback(tibi_dabo_msgs::guiResultPtr& result) 
{ 
  mutex.lock(); 
    //update result data to be sent to client 
    //result->data = data; 
  mutex.unlock(); 
} 

void QNode::gui_actionGetFeedbackCallback(tibi_dabo_msgs::guiFeedbackPtr& feedback) 
{ 
  mutex.lock(); 
    //keep track of feedback
    //ROS_INFO("feedback: %s", feedback->data.c_str()); 
  mutex.unlock(); 
}

//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