Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
navigator_thread.cpp
1 
2 /***************************************************************************
3  * navigator_thread.cpp - Robotino ROS Navigator Thread
4  *
5  * Created: Sat June 09 15:13:27 2012
6  * Copyright 2012 Sebastian Reuter
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "navigator_thread.h"
23 
24 using namespace fawkes;
25 
26 /** @class RosNavigatorThread "navigator_thread.h"
27  * Send Fawkes locomotion commands off to ROS.
28  * @author Sebastian Reuter
29  */
30 
31 /** Contructor. */
33  : Thread("RosNavigatorThread", Thread::OPMODE_WAITFORWAKEUP),
35 {
36 }
37 
38 void
40 {
41  // navigator interface to give feedback of navigation task (writer)
42  try {
43  nav_if_ = blackboard->open_for_writing<NavigatorInterface>("Navigator");
44  } catch (Exception& e) {
45  e.append("%s initialization failed, could not open navigator "
46  "interface for writing", name());
47  logger->log_error(name(), e);
48  throw;
49  }
50 
51  //tell the action client that we want to spin a thread by default
52  ac_ = new MoveBaseClient("move_base", false);
53 
54  logger->log_error(name(),"Change Interface (x,y) ");
55  cmd_sent_ = false;
56  connected_history_ = false;
57 }
58 
59 void
61 {
62  // close interfaces
63  try {
64  blackboard->close(nav_if_);
65  } catch (Exception& e) {
66  logger->log_error(name(), "Closing interface failed!");
67  logger->log_error(name(), e);
68  }
69  delete ac_;
70 }
71 
72 void
73 RosNavigatorThread::check_status()
74 {
75  if(cmd_sent_){
76  if(ac_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
77  nav_if_->set_final(true);
78  nav_if_->set_error_code(0);
79  }
80  else if(ac_->getState() == actionlib::SimpleClientGoalState::ABORTED ||
81  ac_->getState() == actionlib::SimpleClientGoalState::REJECTED){
82  nav_if_->set_final(true);
83  nav_if_->set_error_code(2);
84  }
85  else {
86  nav_if_->set_final(false);
87  nav_if_->set_error_code(0);
88  }
89  nav_if_->write();
90  }
91 }
92 
93 void
94 RosNavigatorThread::send_goal()
95 {
96  //get goal from Navigation interface
97  goal_.target_pose.header.frame_id = "base_link";
98  goal_.target_pose.header.stamp = ros::Time::now();
99  goal_.target_pose.pose.position.x = nav_if_->dest_x();
100  goal_.target_pose.pose.position.y = nav_if_->dest_y();
101  fawkes::tf::Quaternion q(nav_if_->dest_ori(), 0, 0);
102  goal_.target_pose.pose.orientation.x = q.x();
103  goal_.target_pose.pose.orientation.y = q.y();
104  goal_.target_pose.pose.orientation.z = q.z();
105  goal_.target_pose.pose.orientation.w = q.w();
106 
107  ac_->sendGoal(goal_);
108 
109  cmd_sent_ = true;
110 }
111 
112 void
114 {
115  if(ac_->isServerConnected()){
116 
117  connected_history_ = true;
118  // process incoming messages from fawkes
119  while (! nav_if_->msgq_empty()) {
120 
121  // stop
122  if (NavigatorInterface::StopMessage *msg = nav_if_->msgq_first_safe(msg)) {
123  logger->log_info(name(), "Stop message received");
124  nav_if_->set_dest_x(0);
125  nav_if_->set_dest_y(0);
126  nav_if_->set_dest_ori(0);
127 
128  nav_if_->set_msgid(msg->id());
129  }
130 
131  // cartesian goto
132  else if (NavigatorInterface::CartesianGotoMessage *msg = nav_if_->msgq_first_safe(msg)) {
133  logger->log_info(name(), "Cartesian goto message received (x,y) = (%f,%f)",
134  msg->x(), msg->y());
135  nav_if_->set_dest_x(msg->x());
136  nav_if_->set_dest_y(msg->y());
137  nav_if_->set_dest_ori(msg->orientation());
138 
139  nav_if_->set_msgid(msg->id());
140  }
141 
142  // polar goto
143  else if (NavigatorInterface::PolarGotoMessage *msg = nav_if_->msgq_first_safe(msg)) {
144  logger->log_info(name(), "Polar goto message received (phi,dist) = (%f,%f)",
145  msg->phi(), msg->dist());
146  nav_if_->set_dest_x(msg->dist() * cos(msg->phi()));
147  nav_if_->set_dest_y(msg->dist() * cos(msg->phi()));
148  nav_if_->set_dest_ori(msg->phi());
149 
150  nav_if_->set_msgid(msg->id());
151  }
152 
153  // max velocity
154  else if (NavigatorInterface::SetMaxVelocityMessage *msg = nav_if_->msgq_first_safe(msg)) {
155  logger->log_info(name(),"velocity message received %f",msg->max_velocity());
156  nav_if_->set_max_velocity(msg->max_velocity());
157 
158  nav_if_->set_msgid(msg->id());
159  }
160 
161  else if (NavigatorInterface::SetSecurityDistanceMessage *msg = nav_if_->msgq_first_safe(msg)) {
162  logger->log_info(name(),"velocity message received %f",msg->security_distance ());
163  nav_if_->set_security_distance (msg->security_distance ());
164 
165  nav_if_->set_msgid(msg->id());
166  }
167 
168  nav_if_->msgq_pop();
169  nav_if_->write();
170 
171  send_goal();
172  } // while
173 
174  check_status();
175 
176  } // if
177  else{
178  logger->log_info(name(),"ROS-ActionServer is not up yet");
179 
180  if (connected_history_){
181  delete ac_;
182  ac_ = new MoveBaseClient("move_base", false);
183  connected_history_ = false;
184  }
185  }
186 } // function