Fawkes API  Fawkes Development Version
navigator_thread.cpp
00001 
00002 /***************************************************************************
00003  *  navigator_thread.cpp - Robotino ROS Navigator Thread
00004  *
00005  *  Created: Sat June 09 15:13:27 2012
00006  *  Copyright  2012  Sebastian Reuter
00007  ****************************************************************************/
00008 
00009 /*  This program is free software; you can redistribute it and/or modify
00010  *  it under the terms of the GNU General Public License as published by
00011  *  the Free Software Foundation; either version 2 of the License, or
00012  *  (at your option) any later version.
00013  *
00014  *  This program is distributed in the hope that it will be useful,
00015  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00016  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017  *  GNU Library General Public License for more details.
00018  *
00019  *  Read the full text in the LICENSE.GPL file in the doc directory.
00020  */
00021 
00022 #include "navigator_thread.h"
00023 
00024 using namespace fawkes;
00025 
00026 /** @class RosNavigatorThread "navigator_thread.h"
00027  * Send Fawkes locomotion commands off to ROS.
00028  * @author Sebastian Reuter
00029  */
00030 
00031 /** Contructor. */
00032 RosNavigatorThread::RosNavigatorThread()
00033   : Thread("RosNavigatorThread", Thread::OPMODE_WAITFORWAKEUP),
00034     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT)
00035 {
00036 }
00037 
00038 void
00039 RosNavigatorThread::init()
00040 {
00041   // navigator interface to give feedback of navigation task (writer)
00042   try {
00043     nav_if_ = blackboard->open_for_writing<NavigatorInterface>("Navigator");
00044   } catch (Exception& e) {
00045     e.append("%s initialization failed, could not open navigator "
00046              "interface for writing", name());
00047     logger->log_error(name(), e);
00048     throw;
00049   }
00050 
00051   //tell the action client that we want to spin a thread by default
00052   ac_ = new MoveBaseClient("move_base", false);
00053 
00054   logger->log_error(name(),"Change Interface (x,y) ");
00055   cmd_sent_ = false;
00056   connected_history_ = false;
00057 }
00058 
00059 void
00060 RosNavigatorThread::finalize()
00061 {
00062   // close interfaces
00063   try {
00064     blackboard->close(nav_if_);
00065   } catch (Exception& e) {
00066     logger->log_error(name(), "Closing interface failed!");
00067     logger->log_error(name(), e);
00068   }
00069   delete ac_;
00070 }
00071 
00072 void
00073 RosNavigatorThread::check_status()
00074 {
00075   if(cmd_sent_){
00076     if(ac_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
00077       nav_if_->set_final(true);
00078       nav_if_->set_error_code(0);
00079     }
00080     else if(ac_->getState() == actionlib::SimpleClientGoalState::ABORTED ||
00081              ac_->getState() == actionlib::SimpleClientGoalState::REJECTED){
00082       nav_if_->set_final(true);
00083       nav_if_->set_error_code(2);
00084     }
00085     else {
00086       nav_if_->set_final(false);
00087       nav_if_->set_error_code(0);
00088     }
00089     nav_if_->write();
00090   }
00091 }
00092 
00093 void
00094 RosNavigatorThread::send_goal()
00095 {
00096   //get goal from Navigation interface
00097   goal_.target_pose.header.frame_id = "base_link";
00098   goal_.target_pose.header.stamp = ros::Time::now();
00099   goal_.target_pose.pose.position.x = nav_if_->dest_x();
00100   goal_.target_pose.pose.position.y = nav_if_->dest_y();
00101   fawkes::tf::Quaternion q(nav_if_->dest_ori(), 0, 0);
00102   goal_.target_pose.pose.orientation.x = q.x();
00103   goal_.target_pose.pose.orientation.y = q.y();
00104   goal_.target_pose.pose.orientation.z = q.z();
00105   goal_.target_pose.pose.orientation.w = q.w();
00106 
00107   ac_->sendGoal(goal_);
00108 
00109   cmd_sent_ = true;
00110 }
00111 
00112 void
00113 RosNavigatorThread::loop()
00114 {
00115   if(ac_->isServerConnected()){
00116 
00117     connected_history_ = true;
00118     // process incoming messages from fawkes
00119     while (! nav_if_->msgq_empty()) {
00120 
00121       // stop
00122       if (NavigatorInterface::StopMessage *msg = nav_if_->msgq_first_safe(msg)) {
00123         logger->log_info(name(), "Stop message received");
00124         nav_if_->set_dest_x(0);
00125         nav_if_->set_dest_y(0);
00126         nav_if_->set_dest_ori(0);
00127 
00128         nav_if_->set_msgid(msg->id());
00129       }
00130 
00131       // cartesian goto
00132       else if (NavigatorInterface::CartesianGotoMessage *msg = nav_if_->msgq_first_safe(msg)) {
00133         logger->log_info(name(), "Cartesian goto message received (x,y) = (%f,%f)",
00134                          msg->x(), msg->y());
00135         nav_if_->set_dest_x(msg->x());
00136         nav_if_->set_dest_y(msg->y());
00137         nav_if_->set_dest_ori(msg->orientation());
00138 
00139         nav_if_->set_msgid(msg->id());
00140       }
00141 
00142       // polar goto
00143       else if (NavigatorInterface::PolarGotoMessage *msg = nav_if_->msgq_first_safe(msg)) {
00144         logger->log_info(name(), "Polar goto message received (phi,dist) = (%f,%f)",
00145                          msg->phi(), msg->dist());
00146         nav_if_->set_dest_x(msg->dist() * cos(msg->phi()));
00147         nav_if_->set_dest_y(msg->dist() * cos(msg->phi()));
00148         nav_if_->set_dest_ori(msg->phi());
00149 
00150         nav_if_->set_msgid(msg->id());
00151       }
00152 
00153       // max velocity
00154       else if (NavigatorInterface::SetMaxVelocityMessage *msg = nav_if_->msgq_first_safe(msg)) {
00155         logger->log_info(name(),"velocity message received %f",msg->max_velocity());
00156         nav_if_->set_max_velocity(msg->max_velocity());
00157         
00158         nav_if_->set_msgid(msg->id());
00159       }
00160 
00161       else if (NavigatorInterface::SetSecurityDistanceMessage *msg = nav_if_->msgq_first_safe(msg)) {
00162         logger->log_info(name(),"velocity message received %f",msg->security_distance ());
00163         nav_if_->set_security_distance (msg->security_distance ());
00164         
00165         nav_if_->set_msgid(msg->id());
00166       }
00167 
00168       nav_if_->msgq_pop();
00169       nav_if_->write();
00170 
00171       send_goal();
00172     } // while
00173 
00174     check_status();
00175 
00176   } // if
00177   else{
00178     logger->log_info(name(),"ROS-ActionServer is not up yet");
00179 
00180     if (connected_history_){
00181       delete ac_;
00182       ac_ = new MoveBaseClient("move_base", false);
00183       connected_history_ = false;
00184     }
00185   }
00186 } // function