00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include "ual_catec_alg_node.h"
00031
00032 UalCatecAlgNode::UalCatecAlgNode(void) :
00033 algorithm_base::IriBaseAlgorithm<UalCatecAlgorithm>(),
00034 LandGoalSended_(false),
00035 TakeOffGoalSended_(false),
00036 go_initial_tag_pose_(false),
00037 take_off_action_(false),
00038 land_action_(false),
00039 tag_detected_(false),
00040 land_position_ok_(false),
00041 land_position_fixed_(false),
00042 seq_(0),
00043 dist_to_tag_(1.0),
00044 ini_pos_x_(-0.2),
00045 ini_pos_y_(-2.044),
00046 ini_pos_z_(1.3),
00047 ini_pos_yaw_(0.0),
00048 cruise_(0.2),
00049 marker_cruise_(0.1),
00050 yaw_cruise_(0.2),
00051 take_off_action_client_("take_off_action", true),
00052 land_action_client_("land_action", true)
00053 {
00054
00055 this->loop_rate_ = 50;
00056
00057 this->land_pos_=Eigen::MatrixXf::Zero(1,4);
00058 this->last_t_tag_=ros::Time::now();
00059
00060
00061 this->control_references_rw_publisher_ = this->public_node_handle_.advertise<catec_msgs::ControlReferenceRwStamped>("control_references_rw", 1);
00062
00063
00064 this->input_tag_subscriber_ = this->public_node_handle_.subscribe("input_tag", 1, &UalCatecAlgNode::input_tag_callback, this);
00065 this->ual_state_subscriber_ = this->public_node_handle_.subscribe("ual_state", 1, &UalCatecAlgNode::ual_state_callback, this);
00066
00067
00068
00069
00070
00071
00072
00073
00074 }
00075
00076 UalCatecAlgNode::~UalCatecAlgNode(void)
00077 {
00078
00079 }
00080
00081 void UalCatecAlgNode::mainNodeThread(void)
00082 {
00083
00084
00085
00086 double vec_pos, yaw_diff;
00087 int direction;
00088
00089 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));
00090 yaw_diff = abs(this->quad_yaw_ - this->land_pos_(3));
00091
00092 if (this->land_action_ && !this->LandGoalSended_ && !this->land_position_ok_ && vec_pos < 0.15 && yaw_diff < 0.35){
00093 this->land_position_ok_=true;
00094 }
00095
00096
00097
00098
00099
00100
00101
00102 if (this->take_off_action_ && !this->TakeOffGoalSended_ && !this->land_action_ && !this->LandGoalSended_){
00103 if (!this->land_position_fixed_){
00104 this->land_pos_(0)=this->quad_x_;
00105 this->land_pos_(1)=this->quad_y_;
00106 this->land_pos_(2)=this->quad_z_+0.3;
00107 this->land_pos_(3)=this->quad_yaw_;
00108 this->land_position_fixed_=true;
00109
00110 }
00111 else {
00112 take_off_actionMakeActionRequest();
00113 this->TakeOffGoalSended_ = true;
00114 }
00115 }
00116
00117 if (this->land_action_ && !this->LandGoalSended_){
00118 if (!this->land_position_ok_){
00119 this->goal_x_=this->land_pos_(0);
00120 this->goal_y_=this->land_pos_(1);
00121 this->goal_z_=this->land_pos_(2);
00122 this->goal_yaw_=this->land_pos_(3);
00123 this->goal_cruise_=this->cruise_;
00124 }
00125 else {
00126 land_actionMakeActionRequest();
00127 this->LandGoalSended_ = true;
00128 }
00129 }
00130
00131 if (this->go_initial_tag_pose_ && !this->land_action_ && !this->LandGoalSended_){
00132 this->goal_x_=this->ini_pos_x_;
00133 this->goal_y_=this->ini_pos_y_;
00134 this->goal_z_=this->ini_pos_z_;
00135 this->goal_yaw_=this->ini_pos_yaw_;
00136 this->goal_cruise_=this->cruise_;
00137
00138
00139 double obj_from_quad = (this->ini_pos_yaw_ + 2 * 3.14159)-(this->quad_yaw_ + 2 * 3.14159);
00140
00141 direction = 1;
00142 if(obj_from_quad<0)
00143 direction = -1;
00144
00145 if(abs(obj_from_quad) > this->yaw_cruise_)
00146 {
00147 this->goal_yaw_ = this->quad_yaw_ + (this->yaw_cruise_ * direction);
00148 }
00149
00150
00151
00152 }
00153
00154 if (!this->go_initial_tag_pose_ && this->tag_detected_ && !this->land_action_ && !this->LandGoalSended_) {
00155
00156
00157 this->goal_x_=0-this->tag_y_;
00158 this->goal_y_=0-this->tag_x_;
00159
00160
00161 if (this->fixed_to_id_==1) this->goal_z_=0-this->tag_z_+(this->dist_to_tag_)/2;
00162 else this->goal_z_=0-this->tag_z_+this->dist_to_tag_;
00163
00164
00165 this->goal_yaw_=this->quad_yaw_-this->tag_yaw_;
00166 this->goal_cruise_=this->marker_cruise_;
00167
00168 direction = 1;
00169 if(this->tag_yaw_<0)
00170 direction = -1;
00171
00172 if(abs(this->tag_yaw_) > this->yaw_cruise_)
00173 {
00174 this->goal_yaw_ = this->quad_yaw_ - (this->yaw_cruise_ * direction);
00175 }
00176
00177
00178 btVector3 relativePosition;
00179 relativePosition.setX(this->goal_x_);
00180 relativePosition.setY(this->goal_y_);
00181 relativePosition.setZ(this->goal_z_);
00182
00183 btVector3 absolute_position = body_earth_rot_*relativePosition;
00184
00185 btVector3 quadPosition(this->quad_x_,this->quad_y_,this->quad_z_);
00186
00187 absolute_position += quadPosition;
00188
00189
00190 this->goal_x_ = absolute_position.getX();
00191 this->goal_y_ = absolute_position.getY();
00192 this->goal_z_ = absolute_position.getZ();
00193
00194
00195 this->last_t_tag_=ros::Time::now();
00196
00197 }
00198 ros::Duration t_count=ros::Time::now()-this->last_t_tag_;
00199
00200 if (!this->go_initial_tag_pose_ && !this->tag_detected_ && !this->land_action_ && !this->LandGoalSended_ && t_count.toSec()>0.7) {
00201
00202
00203
00204
00205 this->goal_x_=this->ini_pos_x_;
00206 this->goal_y_=this->ini_pos_y_;
00207 this->goal_z_=this->ini_pos_z_;
00208 this->goal_yaw_=this->ini_pos_yaw_;
00209 this->goal_cruise_=this->marker_cruise_;
00210
00211
00212 }
00213
00214 this->ControlReferenceRwStamped_msg_.header.seq=this->seq_;
00215 this->ControlReferenceRwStamped_msg_.header.stamp=ros::Time::now();
00216 this->ControlReferenceRwStamped_msg_.header.frame_id="iri_ual_catec";
00217 this->ControlReferenceRwStamped_msg_.c_reference_rw.position.x=this->goal_x_;
00218 this->ControlReferenceRwStamped_msg_.c_reference_rw.position.y=this->goal_y_;
00219 this->ControlReferenceRwStamped_msg_.c_reference_rw.position.z=this->goal_z_;
00220 this->ControlReferenceRwStamped_msg_.c_reference_rw.heading=this->goal_yaw_;
00221
00222 this->ControlReferenceRwStamped_msg_.c_reference_rw.cruise=this->goal_cruise_;
00223
00224
00225 this->control_references_rw_publisher_.publish(this->ControlReferenceRwStamped_msg_);
00226 }
00227
00228
00229 void UalCatecAlgNode::input_tag_callback(const ar_pose::ARMarkers::ConstPtr& msg)
00230 {
00231
00232
00233
00234
00235 this->input_tag_mutex_.enter();
00236
00237 if (msg->markers.empty()){
00238 this->tag_detected_=false;
00239 }
00240
00241 else if (!msg->markers.empty()){
00242 this->fixed_to_id_=0;
00243 this->tag_detected_=true;
00244 if (1<msg->markers.size()) {
00245 ROS_INFO("Two markers zone \n");
00246 if (msg->markers[0].pose.pose.position.z<(this->dist_to_tag_+0.1)){
00247 this->pose_ = msg->markers[1].pose.pose;
00248 this->t_ = msg->markers[1].header.stamp;
00249 this->seq_ = msg->markers[1].header.seq;
00250 this->fixed_to_id_=1;
00251 ROS_INFO("BAR at z distance: %f \n", msg->markers[1].pose.pose.position.z);
00252 }
00253 else {
00254 this->pose_ = msg->markers[0].pose.pose;
00255 this->t_ = msg->markers[0].header.stamp;
00256 this->seq_ = msg->markers[0].header.seq;
00257 this->fixed_to_id_=0;
00258 ROS_INFO("BASE at z distance: %f \n", msg->markers[0].pose.pose.position.z);
00259 }
00260 }
00261 else {
00262 this->pose_ = msg->markers.back().pose.pose;
00263 this->t_ = msg->markers.back().header.stamp;
00264 this->seq_ = msg->markers.back().header.seq;
00265 this->fixed_to_id_=msg->markers.back().id;
00266 }
00267
00268 tf::Transform tf_pose;
00269 double roll,pitch,yaw;
00270 tf::poseMsgToTF(this->pose_, tf_pose);
00271 tf_pose.getBasis().getRPY(roll,pitch,yaw);
00272
00273
00274 if (this->pose_.position.z<0) this->pose_.position.z=-this->pose_.position.z;
00275
00276 this->tag_x_=this->pose_.position.x;
00277 this->tag_y_=this->pose_.position.y;
00278 this->tag_z_=this->pose_.position.z;
00279 this->tag_yaw_=yaw;
00280 }
00281
00282
00283
00284 this->input_tag_mutex_.exit();
00285 }
00286
00287 void UalCatecAlgNode::ual_state_callback(const catec_msgs::UALStateStamped::ConstPtr& msg)
00288 {
00289
00290
00291
00292
00293 this->ual_state_mutex_.enter();
00294
00295 this->quad_x_=msg->ual_state.dynamic_state.position.x;
00296 this->quad_y_=msg->ual_state.dynamic_state.position.y;
00297 this->quad_z_=msg->ual_state.dynamic_state.position.z;
00298 this->quad_yaw_=msg->ual_state.dynamic_state.orientation.z;
00299
00300
00301 rotateOp::Quaternion q_s2;
00302 q_s2.fromEuler(msg->ual_state.dynamic_state.orientation.x,
00303 msg->ual_state.dynamic_state.orientation.y,
00304 msg->ual_state.dynamic_state.orientation.z,rotateOp::TransformationTypes::EULER123);
00305
00306 btQuaternion q_ros;
00307 q_ros.setW(q_s2.getW());
00308 q_ros.setX(q_s2.getX());
00309 q_ros.setY(q_s2.getY());
00310 q_ros.setZ(q_s2.getZ());
00311
00312 this->body_earth_rot_ = btMatrix3x3(q_ros);
00313
00314
00315
00316 this->ual_state_mutex_.exit();
00317 }
00318
00319
00320
00321
00322 void UalCatecAlgNode::take_off_actionActive()
00323 {
00324 ROS_INFO("UalCatecAlgNode::take_off_actionActive: Goal just went active!");
00325 }
00326 void UalCatecAlgNode::take_off_actionFeedback(const catec_actions_msgs::TakeOffFeedbackConstPtr& feedback)
00327 {
00328 ROS_INFO("UalCatecAlgNode::take_off_actionFeedback: Take Off phase: %d", feedback->phase);
00329 }
00330 void UalCatecAlgNode::take_off_actionDone(const actionlib::SimpleClientGoalState& state, const catec_actions_msgs::TakeOffResultConstPtr& result)
00331 {
00332 ROS_INFO("Take off finished in state [%s]", state.toString().c_str());
00333 ROS_INFO("Take off task return: [%s]", takeOffMessage(result).c_str());
00334
00335 if(state.state_==state.SUCCEEDED || result->result==catec_actions_msgs::TakeOffResult::FLYING || result->result==catec_actions_msgs::TakeOffResult::ALREADY_FLYING)
00336 {
00337 ROS_INFO("UalCatecAlgNode::TakeOff Done");
00338 }
00339 this->TakeOffGoalSended_ = false;
00340 }
00341
00342 void UalCatecAlgNode::land_actionActive()
00343 {
00344 ROS_INFO("UalCatecAlgNode::land_actionActive: Goal just went active!");
00345 }
00346 void UalCatecAlgNode::land_actionFeedback(const catec_actions_msgs::LandFeedbackConstPtr& feedback)
00347 {
00348 ROS_INFO("UalCatecAlgNode::land_actionFeedback: Land phase: %d", feedback->phase);
00349 }
00350 void UalCatecAlgNode::land_actionDone(const actionlib::SimpleClientGoalState& state, const catec_actions_msgs::LandResultConstPtr& result)
00351 {
00352 if( state.toString().compare("SUCCEEDED") == 0 ) {
00353 ROS_INFO("UalCatecAlgNode::land_actionDone: Landed!");
00354 ROS_INFO("UalCatecAlgNode::land_actionDone: Land task return: [%s]", landMessage(result).c_str());
00355 }
00356 else ROS_INFO("UalCatecAlgNode::land_actionDone: %s", state.toString().c_str());
00357 this->LandGoalSended_=false;
00358 this->land_position_fixed_=false;
00359 }
00360
00361
00362 void UalCatecAlgNode::take_off_actionMakeActionRequest()
00363 {
00364 ROS_INFO("UalCatecAlgNode::take_off_actionMakeActionRequest: Starting New Request!");
00365
00366
00367
00368 take_off_action_client_.waitForServer();
00369 ROS_INFO("UalCatecAlgNode::take_off_actionMakeActionRequest: Server is Available!");
00370
00371
00372 catec_actions_msgs::TakeOffGoal take_off_action_goal_;
00373
00374 take_off_action_client_.sendGoal(take_off_action_goal_,
00375 boost::bind(&UalCatecAlgNode::take_off_actionDone, this, _1, _2),
00376 boost::bind(&UalCatecAlgNode::take_off_actionActive, this),
00377 boost::bind(&UalCatecAlgNode::take_off_actionFeedback, this, _1));
00378 ROS_INFO("UalCatecAlgNode::take_off_actionMakeActionRequest: Goal Sent. Wait for Result!");
00379 }
00380 void UalCatecAlgNode::land_actionMakeActionRequest()
00381 {
00382 ROS_INFO("UalCatecAlgNode::land_actionMakeActionRequest: Starting New Request!");
00383
00384
00385
00386 land_action_client_.waitForServer();
00387 ROS_INFO("UalCatecAlgNode::land_actionMakeActionRequest: Server is Available!");
00388
00389
00390 catec_actions_msgs::LandGoal land_action_goal_;
00391
00392 land_action_client_.sendGoal(land_action_goal_,
00393 boost::bind(&UalCatecAlgNode::land_actionDone, this, _1, _2),
00394 boost::bind(&UalCatecAlgNode::land_actionActive, this),
00395 boost::bind(&UalCatecAlgNode::land_actionFeedback, this, _1));
00396 ROS_INFO("UalCatecAlgNode::land_actionMakeActionRequest: Goal Sent. Wait for Result!");
00397 }
00398
00399 std::string UalCatecAlgNode::landMessage(const catec_actions_msgs::LandResultConstPtr& result)
00400 {
00401 switch(result->result)
00402 {
00403 case catec_actions_msgs::LandResult::LANDED:
00404 return string("LANDED");
00405 break;
00406 case catec_actions_msgs::LandResult::ALREADY_LANDED:
00407 return string("ALREADY_LANDED");
00408 break;
00409 case catec_actions_msgs::LandResult::LAND_CANCELED:
00410 return string("LAND_CANCELED");
00411 break;
00412 case catec_actions_msgs::LandResult::TAKINGOFF:
00413 return string("TAKINGOFF");
00414 break;
00415 }
00416 return string("UNKNOWN");
00417 }
00418 std::string UalCatecAlgNode::takeOffMessage(const catec_actions_msgs::TakeOffResultConstPtr& result)
00419 {
00420 switch(result->result)
00421 {
00422 case catec_actions_msgs::TakeOffResult::FLYING:
00423 return string("FLYING");
00424 break;
00425 case catec_actions_msgs::TakeOffResult::ALREADY_FLYING:
00426 return string("ALREADY_FLYING");
00427 break;
00428 case catec_actions_msgs::TakeOffResult::LANDING:
00429 return string("LANDING");
00430 break;
00431 case catec_actions_msgs::TakeOffResult::TAKEOFF_CANCELED:
00432 return string("TAKEOFF_CANCELED");
00433 break;
00434 }
00435 return string("UNKNOWN");
00436 }
00437
00438 void UalCatecAlgNode::node_config_update(Config &config, uint32_t level)
00439 {
00440 this->alg_.lock();
00441
00442 this->go_initial_tag_pose_=config.goinitialtagpose;
00443
00444
00445 this->take_off_action_=config.take_off_action;
00446 ROS_INFO("Getting position");
00447 if(!this->land_action_ && config.land_action)
00448 {
00449 this->land_position_ok_ = false;
00450 ROS_INFO("Getting position22222");
00451 }
00452 this->land_action_=config.land_action;
00453 this->dist_to_tag_=config.dist_to_tag;
00454 this->ini_pos_x_=config.ini_pos_x;
00455 this->ini_pos_y_=config.ini_pos_y;
00456 this->ini_pos_z_=config.ini_pos_z;
00457 this->ini_pos_yaw_=config.ini_pos_yaw;
00458 this->cruise_=config.cruise;
00459 this->marker_cruise_=config.marker_cruise;
00460 this->yaw_cruise_=config.yaw_cruise;
00461 this->alg_.unlock();
00462 }
00463
00464 void UalCatecAlgNode::addNodeDiagnostics(void)
00465 {
00466 }
00467
00468
00469 int main(int argc,char *argv[])
00470 {
00471 return algorithm_base::main<UalCatecAlgNode>(argc, argv, "ual_catec_alg_node");
00472 }