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",
51  name());
52  logger->log_error(name(), e);
53  throw;
54  }
55 
56  cfg_action_topic_ = config->get_string("/ros/navgraph-breakout/action-topic");
57 
58  ac_ = new NavGraphGotoClient(cfg_action_topic_, false);
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(),
79  "Command received while action provider "
80  "not available, ignoring");
81  pp_nav_if_->msgq_flush();
82  }
83 
84  if (was_connected_) {
85  delete ac_;
86  ac_ = new NavGraphGotoClient(cfg_action_topic_, false);
87  was_connected_ = false;
88  }
89 
90  } else {
91  was_connected_ = true;
92 
93  // Check for new incoming commands and process them
94  while (!pp_nav_if_->msgq_empty()) {
95  if (NavigatorInterface::StopMessage *msg = pp_nav_if_->msgq_first_safe(msg)) {
96  if (goal_active_) {
97  ac_->cancelAllGoals();
98  goal_active_ = false;
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(), "Remote navgraph goto failed: %s", result.errmsg.c_str());
132  }
133  }
134  pp_nav_if_->set_final(true);
135  pp_nav_if_->set_error_code(result.errcode);
136  pp_nav_if_->write();
137  goal_active_ = false;
138  } else if (ac_->getState() == actionlib::SimpleClientGoalState::ABORTED
139  || ac_->getState() == actionlib::SimpleClientGoalState::REJECTED) {
140  pp_nav_if_->set_final(true);
141  pp_nav_if_->set_error_code(NavigatorInterface::ERROR_OBSTRUCTION);
142  pp_nav_if_->write();
143  goal_active_ = false;
144  }
145  }
146  }
147 }
fawkes::Interface::msgq_pop
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1182
RosNavgraphBreakoutThread::loop
virtual void loop()
Code to execute in the thread.
Definition: navgraph_breakout_thread.cpp:74
fawkes::Interface::msgq_empty
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1029
fawkes::NavigatorInterface::StopMessage
StopMessage Fawkes BlackBoard Interface Message.
Definition: NavigatorInterface.h:111
fawkes::Interface::msgq_first_safe
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
Definition: interface.h:303
fawkes::NavigatorInterface::set_error_code
void set_error_code(const uint32_t new_error_code)
Set error_code value.
Definition: NavigatorInterface.cpp:476
fawkes::Logger::log_info
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
fawkes::BlockedTimingAspect
Thread aspect to use blocked timing.
Definition: blocked_timing.h:51
fawkes::Thread::name
const char * name() const
Get name of thread.
Definition: thread.h:100
fawkes::Exception::append
void append(const char *format,...)
Append messages to the message list.
Definition: exception.cpp:333
fawkes::NavigatorInterface::set_msgid
void set_msgid(const uint32_t new_msgid)
Set msgid value.
Definition: NavigatorInterface.cpp:410
fawkes::LoggingAspect::logger
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
Close interface.
fawkes::Logger::log_error
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
RosNavgraphBreakoutThread::init
virtual void init()
Initialize the thread.
Definition: navgraph_breakout_thread.cpp:41
fawkes::NavigatorInterface::set_final
void set_final(const bool new_final)
Set final value.
Definition: NavigatorInterface.cpp:442
fawkes
Fawkes library namespace.
RosNavgraphBreakoutThread::RosNavgraphBreakoutThread
RosNavgraphBreakoutThread()
Contructor.
Definition: navgraph_breakout_thread.cpp:34
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
RosNavgraphBreakoutThread::finalize
virtual void finalize()
Finalize the thread.
Definition: navgraph_breakout_thread.cpp:62
fawkes::ConfigurableAspect::config
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
fawkes::NavigatorInterface::PlaceGotoMessage
PlaceGotoMessage Fawkes BlackBoard Interface Message.
Definition: NavigatorInterface.h:377
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
fawkes::Interface::msgq_flush
void msgq_flush()
Flush all messages.
Definition: interface.cpp:1046
fawkes::Interface::write
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
fawkes::NavigatorInterface
NavigatorInterface Fawkes BlackBoard Interface.
Definition: NavigatorInterface.h:34
fawkes::BlackBoard::open_for_writing
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36