Fawkes API
Fawkes Development Version
|
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