darwin_simple_nav_alg_node.cpp

Go to the documentation of this file.

#include "darwin_simple_nav_alg_node.h"
#include <tf/transform_broadcaster.h>

DarwinSimpleNavAlgNode::DarwinSimpleNavAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<DarwinSimpleNavAlgorithm>(),
  target_pose_aserver_(public_node_handle_, "target_pose"),
  tf_listener(ros::Duration(10.0))
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]

  // [init publishers]
  this->cmd_vel_publisher_ = this->public_node_handle_.advertise<geometry_msgs::Twist>("cmd_vel", 100);
  
  // [init subscribers]
  this->current_pose_subscriber_ = this->public_node_handle_.subscribe("current_pose", 100, &DarwinSimpleNavAlgNode::current_pose_callback, this);
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  target_pose_aserver_.registerStartCallback(boost::bind(&DarwinSimpleNavAlgNode::target_poseStartCallback, this, _1)); 
  target_pose_aserver_.registerStopCallback(boost::bind(&DarwinSimpleNavAlgNode::target_poseStopCallback, this)); 
  target_pose_aserver_.registerIsFinishedCallback(boost::bind(&DarwinSimpleNavAlgNode::target_poseIsFinishedCallback, this)); 
  target_pose_aserver_.registerHasSucceedCallback(boost::bind(&DarwinSimpleNavAlgNode::target_poseHasSucceedCallback, this)); 
  target_pose_aserver_.registerGetResultCallback(boost::bind(&DarwinSimpleNavAlgNode::target_poseGetResultCallback, this, _1)); 
  target_pose_aserver_.registerGetFeedbackCallback(boost::bind(&DarwinSimpleNavAlgNode::target_poseGetFeedbackCallback, this, _1)); 
  
  // [init action clients]

  this->finished=true;
}

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

void DarwinSimpleNavAlgNode::mainNodeThread(void)
{
  // [fill msg structures]
  //this->Twist_msg_.data = my_var;
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]

  // [publish messages]
}

/*  [subscriber callbacks] */
void DarwinSimpleNavAlgNode::current_pose_callback(const nav_msgs::Odometry::ConstPtr& msg) 
{ 
  geometry_msgs::PoseStamped current_pose;
  T2DPose target_pose;
  static bool first=true;
  double vt=0.0,vr=0.0;
  bool tf_exists;

  // the first time an odometry message is received, start the action server
  if(first)
  {
    target_pose_aserver_.start();
    first=false;
  }

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

  //std::cout << msg->data << std::endl; 
  // store the current pose
  try{
    // save the current pose of the robot
    this->current_pose.header=msg->header;
    this->current_pose.pose=msg->pose.pose;
    // compute the platform velocities
    if(!this->finished)
    {
      // translate the target goal to the current MP_BODY frame
      this->target_pose.header.stamp=ros::Time::now();
      tf_exists = tf_listener.waitForTransform(this->target_pose.header.frame_id,std::string("/MP_BODY"),this->target_pose.header.stamp ,
                                               ros::Duration(10),ros::Duration(0.01));
      tf_listener.transformPose(std::string("/MP_BODY"),this->target_pose,current_pose);
      // generate the updated target goal
      target_pose.x=current_pose.pose.position.x;
      target_pose.y=current_pose.pose.position.y;
      target_pose.theta=atan2(target_pose.y,target_pose.x);
      ROS_INFO("Target pos: x: %f, y: %f, theta: %f",target_pose.x,target_pose.y,target_pose.theta);

      if(this->alg_.compute_robot_speed(target_pose,vt,vr))
        this->finished=true;
      ROS_INFO("vt: %f, vr: %f",vt,vr);
      // create the twist
      this->Twist_msg_.linear.x=vt;
      this->Twist_msg_.linear.y=0.0;
      this->Twist_msg_.linear.z=0.0;
      this->Twist_msg_.angular.x=0.0;
      this->Twist_msg_.angular.y=0.0;
      this->Twist_msg_.angular.z=vr;
      // send the twist
      this->cmd_vel_publisher_.publish(this->Twist_msg_);
      //unlock previously blocked shared variables 
      //this->alg_.unlock(); 
    }
  }catch(tf::TransformException ex){
    ROS_ERROR("NoCollisionAlgNode:: %s",ex.what());
  }

  this->current_pose_mutex_.exit(); 
}

/*  [service callbacks] */

/*  [action callbacks] */
void DarwinSimpleNavAlgNode::target_poseStartCallback(const move_base_msgs::MoveBaseGoalConstPtr& goal)
{ 
  geometry_msgs::PoseStamped local_goal;
  bool tf_exists;

  alg_.lock(); 
    //check goal 
  // transform the goal the the MP_BODY frame
  try{
    tf_exists = tf_listener.waitForTransform(goal->target_pose.header.frame_id,std::string("/odom"), goal->target_pose.header.stamp,
                                             ros::Duration(10),ros::Duration(0.01));
    tf_listener.transformPose(std::string("/odom"),goal->target_pose,this->target_pose);

    //save the target position
    this->finished=false;
  }catch(tf::TransformException ex){
    ROS_ERROR("NoCollisionAlgNode:: %s",ex.what());
  }
    //execute goal 
  alg_.unlock(); 
} 

void DarwinSimpleNavAlgNode::target_poseStopCallback(void) 
{ 
  alg_.lock(); 
    //stop action 
  alg_.unlock(); 
} 

bool DarwinSimpleNavAlgNode::target_poseIsFinishedCallback(void) 
{ 
  bool ret = false; 

  alg_.lock(); 
    //if action has finish for any reason 
  ret=this->finished;
    //ret = true; 
  alg_.unlock(); 

  return ret; 
} 

bool DarwinSimpleNavAlgNode::target_poseHasSucceedCallback(void) 
{ 
  bool ret = false; 

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

  return ret; 
} 

void DarwinSimpleNavAlgNode::target_poseGetResultCallback(move_base_msgs::MoveBaseResultPtr& result) 
{ 
  alg_.lock(); 
    //update result data to be sent to client 
    //result->data = data; 
  alg_.unlock(); 
} 

void DarwinSimpleNavAlgNode::target_poseGetFeedbackCallback(move_base_msgs::MoveBaseFeedbackPtr& feedback) 
{
  alg_.lock(); 
    //keep track of feedback 
  // return the current platform position
  this->current_pose_mutex_.enter();
  feedback->base_position.header.seq=0;
  feedback->base_position.header.stamp=ros::Time::now();
  feedback->base_position.header.frame_id="/odom";
  feedback->base_position.pose=this->current_pose.pose;
  this->current_pose_mutex_.exit();
    //ROS_INFO("feedback: %s", feedback->data.c_str()); 
  alg_.unlock(); 
}

/*  [action requests] */

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

  this->alg_.unlock();
}

void DarwinSimpleNavAlgNode::addNodeDiagnostics(void)
{
}

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