darwin_simple_nav_client_alg_node.cpp

Go to the documentation of this file.

#include "darwin_simple_nav_client_alg_node.h"

DarwinSimpleNavClientAlgNode::DarwinSimpleNavClientAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<DarwinSimpleNavClientAlgorithm>(),
  move_base_client_("move_base", true)
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]

  // [init publishers]
  
  // [init subscribers]
  this->target_pose_subscriber_ = this->public_node_handle_.subscribe("target_pose", 100, &DarwinSimpleNavClientAlgNode::target_pose_callback, this);
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
  this->motion_done=true;
}

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

void DarwinSimpleNavClientAlgNode::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]
}

/*  [subscriber callbacks] */
void DarwinSimpleNavClientAlgNode::target_pose_callback(const geometry_msgs::PoseStamped::ConstPtr& msg) 
{ 
  ROS_INFO("DarwinSimpleNavClientAlgNode::target_pose_callback: New Message Received"); 

  //use appropiate mutex to shared variables if necessary 
  //this->alg_.lock(); 
  //this->target_pose_mutex_.enter(); 

  //std::cout << msg->data << std::endl; 
  if(motion_done)
  {
    move_base_goal_.target_pose.header=msg->header;
    move_base_goal_.target_pose.pose.position.x=msg->pose.position.x;
    move_base_goal_.target_pose.pose.position.y=msg->pose.position.y;
    move_base_goal_.target_pose.pose.position.z=0.0;
    move_base_goal_.target_pose.pose.orientation=msg->pose.orientation;
    move_baseMakeActionRequest();
  }

  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  //this->target_pose_mutex_.exit(); 
}

/*  [service callbacks] */

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

  //copy & work with requested result 
  this->motion_done=true;
} 

void DarwinSimpleNavClientAlgNode::move_baseActive() 
{ 
  //ROS_INFO("DarwinSimpleNavClientAlgNode::move_baseActive: Goal just went active!"); 
} 

void DarwinSimpleNavClientAlgNode::move_baseFeedback(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("DarwinSimpleNavClientAlgNode::move_baseFeedback: Got Feedback!"); 

  bool feedback_is_ok = true; 

  //analyze feedback 
  //my_var = feedback->var; 
  std::cout << "Current position : (" << feedback->base_position.pose.position.x << "," << feedback->base_position.pose.position.y << ")" << std::endl;

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

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

  //wait for the action server to start 
  //will wait for infinite time 
  move_base_client_.waitForServer();  
  ROS_INFO("DarwinSimpleNavClientAlgNode::move_baseMakeActionRequest: Server is Available!"); 

  this->motion_done=false;
  //send a goal to the action 
  //move_base_goal_.data = my_desired_goal; 
  move_base_client_.sendGoal(move_base_goal_, 
              boost::bind(&DarwinSimpleNavClientAlgNode::move_baseDone,     this, _1, _2), 
              boost::bind(&DarwinSimpleNavClientAlgNode::move_baseActive,   this), 
              boost::bind(&DarwinSimpleNavClientAlgNode::move_baseFeedback, this, _1)); 
  ROS_INFO("DarwinSimpleNavClientAlgNode::move_baseMakeActionRequest: Goal Sent. Wait for Result!"); 
}

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

  this->alg_.unlock();
}

void DarwinSimpleNavClientAlgNode::addNodeDiagnostics(void)
{
}

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