ual_catec_alg_node.cpp

Go to the documentation of this file.

//=================================================================================================
// Copyright (c) 2012, Àngel Santamaria-Navarro, IRI-UPC-CSIC
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the Mobile Robotics group,
//       Institut de robòtica i infomàtica industrial (IRI), 
//       nor the names of its contributors may be used to
//       endorse or promote products derived from this software without
//       specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================

#include "ual_catec_alg_node.h"

UalCatecAlgNode::UalCatecAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<UalCatecAlgorithm>(),
  LandGoalSended_(false),
  TakeOffGoalSended_(false),
  go_initial_tag_pose_(false),
  take_off_action_(false),
  land_action_(false),
  tag_detected_(false),
  land_position_ok_(false),
  land_position_fixed_(false),
  seq_(0),
  dist_to_tag_(1.0),
  ini_pos_x_(-0.2),
  ini_pos_y_(-2.044),
  ini_pos_z_(1.3),
  ini_pos_yaw_(0.0),
  cruise_(0.2),
  marker_cruise_(0.1),
  yaw_cruise_(0.2),
  take_off_action_client_("take_off_action", true),
  land_action_client_("land_action", true)
{
  //init class attributes if necessary
  this->loop_rate_ = 50;//in [Hz]

  this->land_pos_=Eigen::MatrixXf::Zero(1,4);
  this->last_t_tag_=ros::Time::now();
    
  // [init publishers]
  this->control_references_rw_publisher_ = this->public_node_handle_.advertise<catec_msgs::ControlReferenceRwStamped>("control_references_rw", 1);
  
  // [init subscribers]
  this->input_tag_subscriber_ = this->public_node_handle_.subscribe("input_tag", 1, &UalCatecAlgNode::input_tag_callback, this);
  this->ual_state_subscriber_ = this->public_node_handle_.subscribe("ual_state", 1, &UalCatecAlgNode::ual_state_callback, this);
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

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

void UalCatecAlgNode::mainNodeThread(void)
{
//   this->land_pos_=Eigen::MatrixXf::Zero(1,4);

  // check if the quadrotor is in the land position
  double vec_pos, yaw_diff;
  int direction;
  
  vec_pos=std::sqrt(pow(this->land_pos_(0)-this->quad_x_,2)+pow(this->land_pos_(1)-this->quad_y_,2)+pow(this->land_pos_(2)-this->quad_z_,2));
  yaw_diff = abs(this->quad_yaw_ - this->land_pos_(3));
  
  if (this->land_action_ && !this->LandGoalSended_ && !this->land_position_ok_ && vec_pos < 0.15 && yaw_diff < 0.35){
    this->land_position_ok_=true;
  }
  
  // [fill msg structures]
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]
  //TakeOff
  if (this->take_off_action_ && !this->TakeOffGoalSended_ && !this->land_action_ && !this->LandGoalSended_){
    if (!this->land_position_fixed_){
      this->land_pos_(0)=this->quad_x_;
      this->land_pos_(1)=this->quad_y_;
      this->land_pos_(2)=this->quad_z_+0.3;
      this->land_pos_(3)=this->quad_yaw_;
      this->land_position_fixed_=true;      
      //ROS_INFO("GOT LAND POSITION");
    }
    else {
    take_off_actionMakeActionRequest();
    this->TakeOffGoalSended_ = true;
    }
  }
  //Land
  if (this->land_action_ && !this->LandGoalSended_){
    if (!this->land_position_ok_){
      this->goal_x_=this->land_pos_(0);
      this->goal_y_=this->land_pos_(1);
      this->goal_z_=this->land_pos_(2);
      this->goal_yaw_=this->land_pos_(3);
      this->goal_cruise_=this->cruise_;
    }
    else {
    land_actionMakeActionRequest();
    this->LandGoalSended_ = true;
    }
  }
  //Initial position
  if (this->go_initial_tag_pose_ && !this->land_action_ && !this->LandGoalSended_){
    this->goal_x_=this->ini_pos_x_;
    this->goal_y_=this->ini_pos_y_;
    this->goal_z_=this->ini_pos_z_;
    this->goal_yaw_=this->ini_pos_yaw_;
    this->goal_cruise_=this->cruise_;
    
    // smooth yaw movement
    double obj_from_quad = (this->ini_pos_yaw_ + 2 * 3.14159)-(this->quad_yaw_ + 2 * 3.14159);
    
    direction = 1;
    if(obj_from_quad<0)
      direction = -1;

    if(abs(obj_from_quad) > this->yaw_cruise_)
    {
      this->goal_yaw_ = this->quad_yaw_ + (this->yaw_cruise_ * direction);
    }    
    
    
    //ROS_INFO("*****************INITIAL POSITION************************");
  }
  //Tag
  if (!this->go_initial_tag_pose_ && this->tag_detected_ && !this->land_action_ && !this->LandGoalSended_) {
    
    //Supose quadrotor at origin.
    this->goal_x_=0-this->tag_y_;
    this->goal_y_=0-this->tag_x_;
    
    //Different approaching depending on marker
    if (this->fixed_to_id_==1) this->goal_z_=0-this->tag_z_+(this->dist_to_tag_)/2;
    else this->goal_z_=0-this->tag_z_+this->dist_to_tag_;
    
    
    this->goal_yaw_=this->quad_yaw_-this->tag_yaw_;
    this->goal_cruise_=this->marker_cruise_;
    
    direction = 1;
    if(this->tag_yaw_<0)
      direction = -1;
    
    if(abs(this->tag_yaw_) > this->yaw_cruise_)
    {
      this->goal_yaw_ = this->quad_yaw_ - (this->yaw_cruise_ * direction);
    }
    
    //Rotate position
    btVector3 relativePosition;
    relativePosition.setX(this->goal_x_);
    relativePosition.setY(this->goal_y_);
    relativePosition.setZ(this->goal_z_);
    
    btVector3 absolute_position = body_earth_rot_*relativePosition;
    
    btVector3 quadPosition(this->quad_x_,this->quad_y_,this->quad_z_);
    
    absolute_position += quadPosition;
    
    //Set goal to absolute_position
    this->goal_x_ = absolute_position.getX();
    this->goal_y_ = absolute_position.getY();
    this->goal_z_ = absolute_position.getZ();
    
    
    this->last_t_tag_=ros::Time::now();
    //ROS_INFO("***TAG*******");
  }
  ros::Duration t_count=ros::Time::now()-this->last_t_tag_;
  //Losing tag
  if (!this->go_initial_tag_pose_ && !this->tag_detected_ && !this->land_action_ && !this->LandGoalSended_ && t_count.toSec()>0.7) {
//     this->goal_x_=this->quad_x_;
//     this->goal_y_=this->quad_y_;
//     this->goal_z_=this->quad_z_;
//     this->goal_yaw_=this->quad_yaw_;
    this->goal_x_=this->ini_pos_x_;
    this->goal_y_=this->ini_pos_y_;
    this->goal_z_=this->ini_pos_z_;
    this->goal_yaw_=this->ini_pos_yaw_;
    this->goal_cruise_=this->marker_cruise_;
    //ROS_WARN("****NOTHING*******");
  
  }
  
  this->ControlReferenceRwStamped_msg_.header.seq=this->seq_;
  this->ControlReferenceRwStamped_msg_.header.stamp=ros::Time::now();
  this->ControlReferenceRwStamped_msg_.header.frame_id="iri_ual_catec";
  this->ControlReferenceRwStamped_msg_.c_reference_rw.position.x=this->goal_x_;
  this->ControlReferenceRwStamped_msg_.c_reference_rw.position.y=this->goal_y_;
  this->ControlReferenceRwStamped_msg_.c_reference_rw.position.z=this->goal_z_;
  this->ControlReferenceRwStamped_msg_.c_reference_rw.heading=this->goal_yaw_;
//  this->ControlReferenceRwStamped_msg_.c_reference_rw.heading=0.0;
  this->ControlReferenceRwStamped_msg_.c_reference_rw.cruise=this->goal_cruise_;
  
  // [publish messages]
  this->control_references_rw_publisher_.publish(this->ControlReferenceRwStamped_msg_);
}

/*  [subscriber callbacks] */
void UalCatecAlgNode::input_tag_callback(const ar_pose::ARMarkers::ConstPtr& msg) 
{ 
  //ROS_INFO("UalCatecAlgNode::input_tag_callback: New Message Received"); 

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

  if (msg->markers.empty()){
    this->tag_detected_=false;
  }
  
  else if (!msg->markers.empty()){
    this->fixed_to_id_=0;    
    this->tag_detected_=true;
    if (1<msg->markers.size()) {
      ROS_INFO("Two markers zone \n");
      if (msg->markers[0].pose.pose.position.z<(this->dist_to_tag_+0.1)){
        this->pose_ = msg->markers[1].pose.pose;
        this->t_ = msg->markers[1].header.stamp;
        this->seq_ = msg->markers[1].header.seq;
        this->fixed_to_id_=1;
        ROS_INFO("BAR at z distance: %f \n", msg->markers[1].pose.pose.position.z);
      }
      else {
        this->pose_ = msg->markers[0].pose.pose;
        this->t_ = msg->markers[0].header.stamp;
        this->seq_ = msg->markers[0].header.seq;
        this->fixed_to_id_=0;
        ROS_INFO("BASE at z distance: %f \n", msg->markers[0].pose.pose.position.z);      
      } 
    }
    else {
      this->pose_ = msg->markers.back().pose.pose;
      this->t_ = msg->markers.back().header.stamp;
      this->seq_ = msg->markers.back().header.seq;
      this->fixed_to_id_=msg->markers.back().id;
    }

    tf::Transform tf_pose;
    double roll,pitch,yaw;
    tf::poseMsgToTF(this->pose_, tf_pose);
    tf_pose.getBasis().getRPY(roll,pitch,yaw);
          
    //Avoid detector ambiguity
    if (this->pose_.position.z<0) this->pose_.position.z=-this->pose_.position.z;
    
    this->tag_x_=this->pose_.position.x;
    this->tag_y_=this->pose_.position.y;
    this->tag_z_=this->pose_.position.z;
    this->tag_yaw_=yaw;
  }
  
  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  this->input_tag_mutex_.exit(); 
}

void UalCatecAlgNode::ual_state_callback(const catec_msgs::UALStateStamped::ConstPtr& msg) 
{ 
  //ROS_INFO("UalCatecAlgNode::ual_state_callback: New Message Received"); 

  //use appropiate mutex to shared variables if necessary 
  //this->alg_.lock(); 
  this->ual_state_mutex_.enter(); 
  
  this->quad_x_=msg->ual_state.dynamic_state.position.x;
  this->quad_y_=msg->ual_state.dynamic_state.position.y;
  this->quad_z_=msg->ual_state.dynamic_state.position.z;
  this->quad_yaw_=msg->ual_state.dynamic_state.orientation.z;
  
  
  rotateOp::Quaternion q_s2;
  q_s2.fromEuler(msg->ual_state.dynamic_state.orientation.x,
                 msg->ual_state.dynamic_state.orientation.y,
                 msg->ual_state.dynamic_state.orientation.z,rotateOp::TransformationTypes::EULER123);

  btQuaternion q_ros;
  q_ros.setW(q_s2.getW());
  q_ros.setX(q_s2.getX());
  q_ros.setY(q_s2.getY());
  q_ros.setZ(q_s2.getZ());

  this->body_earth_rot_ = btMatrix3x3(q_ros);
  
  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  this->ual_state_mutex_.exit(); 
}

/*  [service callbacks] */

/*  [action callbacks] */
void UalCatecAlgNode::take_off_actionActive() 
{ 
  ROS_INFO("UalCatecAlgNode::take_off_actionActive: Goal just went active!"); 
} 
void UalCatecAlgNode::take_off_actionFeedback(const catec_actions_msgs::TakeOffFeedbackConstPtr& feedback) 
{ 
  ROS_INFO("UalCatecAlgNode::take_off_actionFeedback: Take Off phase: %d", feedback->phase); 
}
void UalCatecAlgNode::take_off_actionDone(const actionlib::SimpleClientGoalState& state,  const catec_actions_msgs::TakeOffResultConstPtr& result) 
{ 
  ROS_INFO("Take off finished in state [%s]", state.toString().c_str());
  ROS_INFO("Take off task return: [%s]", takeOffMessage(result).c_str());

  if(state.state_==state.SUCCEEDED || result->result==catec_actions_msgs::TakeOffResult::FLYING || result->result==catec_actions_msgs::TakeOffResult::ALREADY_FLYING)
  {
    ROS_INFO("UalCatecAlgNode::TakeOff Done"); 
  }
  this->TakeOffGoalSended_ = false;
} 

void UalCatecAlgNode::land_actionActive() 
{ 
  ROS_INFO("UalCatecAlgNode::land_actionActive: Goal just went active!"); 
} 
void UalCatecAlgNode::land_actionFeedback(const catec_actions_msgs::LandFeedbackConstPtr& feedback) 
{ 
  ROS_INFO("UalCatecAlgNode::land_actionFeedback: Land phase: %d", feedback->phase); 
}
void UalCatecAlgNode::land_actionDone(const actionlib::SimpleClientGoalState& state,  const catec_actions_msgs::LandResultConstPtr& result) 
{ 
  if( state.toString().compare("SUCCEEDED") == 0 ) {
    ROS_INFO("UalCatecAlgNode::land_actionDone: Landed!"); 
    ROS_INFO("UalCatecAlgNode::land_actionDone: Land task return: [%s]", landMessage(result).c_str());
  }
  else ROS_INFO("UalCatecAlgNode::land_actionDone: %s", state.toString().c_str()); 
  this->LandGoalSended_=false;
  this->land_position_fixed_=false;
} 

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

  //wait for the action server to start 
  //will wait for infinite time 
  take_off_action_client_.waitForServer();  
  ROS_INFO("UalCatecAlgNode::take_off_actionMakeActionRequest: Server is Available!"); 

  //send a goal to the action 
  catec_actions_msgs::TakeOffGoal take_off_action_goal_;
  
  take_off_action_client_.sendGoal(take_off_action_goal_, 
              boost::bind(&UalCatecAlgNode::take_off_actionDone,     this, _1, _2), 
              boost::bind(&UalCatecAlgNode::take_off_actionActive,   this), 
              boost::bind(&UalCatecAlgNode::take_off_actionFeedback, this, _1)); 
  ROS_INFO("UalCatecAlgNode::take_off_actionMakeActionRequest: Goal Sent. Wait for Result!"); 
}
void UalCatecAlgNode::land_actionMakeActionRequest() 
{ 
  ROS_INFO("UalCatecAlgNode::land_actionMakeActionRequest: Starting New Request!"); 

  //wait for the action server to start 
  //will wait for infinite time 
  land_action_client_.waitForServer();  
  ROS_INFO("UalCatecAlgNode::land_actionMakeActionRequest: Server is Available!"); 

  //send a goal to the action 
  catec_actions_msgs::LandGoal land_action_goal_;
  
  land_action_client_.sendGoal(land_action_goal_, 
              boost::bind(&UalCatecAlgNode::land_actionDone,     this, _1, _2), 
              boost::bind(&UalCatecAlgNode::land_actionActive,   this), 
              boost::bind(&UalCatecAlgNode::land_actionFeedback, this, _1)); 
  ROS_INFO("UalCatecAlgNode::land_actionMakeActionRequest: Goal Sent. Wait for Result!"); 
}

std::string UalCatecAlgNode::landMessage(const catec_actions_msgs::LandResultConstPtr& result)
{
        switch(result->result)
        {
          case catec_actions_msgs::LandResult::LANDED:
                        return string("LANDED");
                        break;
                case catec_actions_msgs::LandResult::ALREADY_LANDED:
                        return string("ALREADY_LANDED");
                        break;
                case catec_actions_msgs::LandResult::LAND_CANCELED:
                        return string("LAND_CANCELED");
                        break;
                case catec_actions_msgs::LandResult::TAKINGOFF:
                        return string("TAKINGOFF");
                        break;
        }
        return string("UNKNOWN");
}
std::string UalCatecAlgNode::takeOffMessage(const catec_actions_msgs::TakeOffResultConstPtr& result)
{
        switch(result->result)
        {
                case catec_actions_msgs::TakeOffResult::FLYING:
                        return string("FLYING");
                        break;
                case catec_actions_msgs::TakeOffResult::ALREADY_FLYING:
                        return string("ALREADY_FLYING");
                        break;
                case catec_actions_msgs::TakeOffResult::LANDING:
                        return string("LANDING");
                        break;
                case catec_actions_msgs::TakeOffResult::TAKEOFF_CANCELED:
                        return string("TAKEOFF_CANCELED");
                        break;
        }
        return string("UNKNOWN");
}

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

  this->go_initial_tag_pose_=config.goinitialtagpose;
  
  
  this->take_off_action_=config.take_off_action;
  ROS_INFO("Getting position");
  if(!this->land_action_ && config.land_action)
  {
    this->land_position_ok_ = false;
    ROS_INFO("Getting position22222");
  }
  this->land_action_=config.land_action;  
  this->dist_to_tag_=config.dist_to_tag;
  this->ini_pos_x_=config.ini_pos_x;
  this->ini_pos_y_=config.ini_pos_y;
  this->ini_pos_z_=config.ini_pos_z;
  this->ini_pos_yaw_=config.ini_pos_yaw;
  this->cruise_=config.cruise;
  this->marker_cruise_=config.marker_cruise;
  this->yaw_cruise_=config.yaw_cruise;
  this->alg_.unlock();
}

void UalCatecAlgNode::addNodeDiagnostics(void)
{
}

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