Fawkes API  Fawkes Development Version
navgraph_breakout_thread.cpp
1 
2 /***************************************************************************
3  * navgraph_breakout_thread.cpp - Provide navgraph-like API through ROS
4  *
5  * Created: Fri Jan 27 11:35:39 2017
6  * Copyright 2017 Tim Niemueller
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 "navgraph_breakout_thread.h"
23 
24 #include <interfaces/NavigatorInterface.h>
25 
26 using namespace fawkes;
27 
28 /** @class RosNavgraphBreakoutThread "navigator_thread.h"
29  * Provide navgraph-like API through ROS.
30  * @author Tim Niemueller
31  */
32 
33 /** Contructor. */
35  : Thread("RosNavgraphBreakoutThread", Thread::OPMODE_WAITFORWAKEUP),
37 {
38 }
39 
40 void
42 {
43  goal_active_ = false;
44  was_connected_ = false;
45 
46  try {
47  pp_nav_if_ = blackboard->open_for_writing<NavigatorInterface>("Pathplan");
48  } catch (Exception& e) {
49  e.append("%s initialization failed, could not open navigator "
50  "interface for writing", name());
51  logger->log_error(name(), e);
52  throw;
53  }
54 
55  cfg_action_topic_ = config->get_string("/ros/navgraph-breakout/action-topic");
56 
57  ac_ = new NavGraphGotoClient(cfg_action_topic_, false);
58 
59 }
60 
61 void
63 {
64  try {
65  blackboard->close(pp_nav_if_);
66  } catch (Exception& e) {
67  logger->log_error(name(), "Closing interface failed!");
68  logger->log_error(name(), e);
69  }
70  delete ac_;
71 }
72 
73 void
75 {
76  if (! ac_->isServerConnected()) {
77  if (! pp_nav_if_->msgq_empty()) {
78  logger->log_warn(name(), "Command received while action provider "
79  "not available, ignoring");
80  pp_nav_if_->msgq_flush();
81  }
82 
83  if (was_connected_){
84  delete ac_;
85  ac_ = new NavGraphGotoClient(cfg_action_topic_, false);
86  was_connected_ = false;
87  }
88 
89  } else {
90  was_connected_ = true;
91 
92  // Check for new incoming commands and process them
93  while (! pp_nav_if_->msgq_empty()) {
94  if (NavigatorInterface::StopMessage *msg = pp_nav_if_->msgq_first_safe(msg)) {
95  if (goal_active_) {
96  ac_->cancelAllGoals();
97  goal_active_ = false;
98  }
99  }
100  else if (NavigatorInterface::PlaceGotoMessage *msg = pp_nav_if_->msgq_first_safe(msg)) {
101  logger->log_info(name(), "Relaying place goto for %s", msg->place());
102 
103  pp_nav_if_->set_msgid(msg->id());
104 
105  goal_.place = msg->place();
106  goal_.orientation = std::numeric_limits<float>::quiet_NaN();
107  if (goal_active_) {
108  ac_->cancelAllGoals();
109  }
110  ac_->sendGoal(goal_);
111 
112  pp_nav_if_->set_final(false);
113  pp_nav_if_->set_error_code(0);
114  pp_nav_if_->write();
115  goal_active_ = true;
116  }
117 
118  pp_nav_if_->msgq_pop();
119  }
120 
121  // If there is an active goal, check for its completion
122  if (goal_active_){
123  if (ac_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
124  fawkes_msgs::NavGraphGotoResult result = *( ac_->getResult() );
125  if (result.errcode != fawkes_msgs::NavGraphGotoResult::ERROR_NONE) {
126  if (result.errmsg.empty()) {
127  logger->log_warn(name(),
128  "Remote navgraph goto failed without error message (code %u)",
129  result.errcode);
130  } else {
131  logger->log_warn(name(),
132  "Remote navgraph goto failed: %s", result.errmsg.c_str());
133  }
134  }
135  pp_nav_if_->set_final(true);
136  pp_nav_if_->set_error_code(result.errcode);
137  pp_nav_if_->write();
138  goal_active_ = false;
139  } else if (ac_->getState() == actionlib::SimpleClientGoalState::ABORTED ||
140  ac_->getState() == actionlib::SimpleClientGoalState::REJECTED)
141  {
142  pp_nav_if_->set_final(true);
143  pp_nav_if_->set_error_code(NavigatorInterface::ERROR_OBSTRUCTION);
144  pp_nav_if_->write();
145  goal_active_ = false;
146  }
147  }
148  }
149 }
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1048
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Fawkes library namespace.
void set_final(const bool new_final)
Set final value.
Thread class encapsulation of pthreads.
Definition: thread.h:42
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:500
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
virtual void loop()
Code to execute in the thread.
Thread aspect to use blocked timing.
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1193
PlaceGotoMessage Fawkes BlackBoard Interface Message.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual void init()
Initialize the thread.
const char * name() const
Get name of thread.
Definition: thread.h:95
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
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.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
Definition: interface.h:302
void msgq_flush()
Flush all messages.
Definition: interface.cpp:1064
void set_msgid(const uint32_t new_msgid)
Set msgid value.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
void append(const char *format,...)
Append messages to the message list.
Definition: exception.cpp:341
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
virtual void finalize()
Finalize the thread.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
StopMessage Fawkes BlackBoard Interface Message.
NavigatorInterface Fawkes BlackBoard Interface.
virtual void close(Interface *interface)=0
Close interface.