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
virtual void finalize()
Finalize the thread.
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:972
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
void set_final(const bool new_final)
Set final value.
float dest_y() const
Get dest_y value.
RosNavigatorThread()
Contructor.
SetSecurityDistanceMessage Fawkes BlackBoard Interface Message.
PolarGotoMessage Fawkes BlackBoard Interface Message.
Thread class encapsulation of pthreads.
Definition: thread.h:42
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:495
float dest_ori() const
Get dest_ori value.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
void set_security_distance(const float new_security_distance)
Set security_distance value.
float dest_x() const
Get dest_x value.
Thread aspect to use blocked timing.
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1118
virtual void init()
Initialize the thread.
Base class for exceptions in Fawkes.
Definition: exception.h:36
CartesianGotoMessage Fawkes BlackBoard Interface Message.
virtual void loop()
Code to execute in the thread.
void set_max_velocity(const float new_max_velocity)
Set max_velocity value.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
const char * name() const
Get name of thread.
Definition: thread.h:95
void set_dest_ori(const float new_dest_ori)
Set dest_ori value.
void set_dest_x(const float new_dest_x)
Set dest_x value.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
Definition: interface.h:293
void set_dest_y(const float new_dest_y)
Set dest_y value.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
SetMaxVelocityMessage Fawkes BlackBoard Interface Message.
void append(const char *format,...)
Append messages to the message list.
Definition: exception.cpp:341
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:43
StopMessage Fawkes BlackBoard Interface Message.
NavigatorInterface Fawkes BlackBoard Interface.
virtual void close(Interface *interface)=0
Close interface.