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
26using 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
40void
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
61void
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
73void
75{
76 if (!ac_->isServerConnected()) {
77 if (!pp_nav_if_->msgq_empty()) {
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()) {
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}
virtual void init()
Initialize the thread.
virtual void loop()
Code to execute in the thread.
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
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void append(const char *format,...) noexcept
Append messages to the message list.
Definition: exception.cpp:333
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
void msgq_flush()
Flush all messages.
Definition: interface.cpp:1079
MessageType * msgq_first_safe(MessageType *&msg) noexcept
Get first message casted to the desired type without exceptions.
Definition: interface.h:340
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.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
PlaceGotoMessage Fawkes BlackBoard Interface Message.
StopMessage Fawkes BlackBoard Interface Message.
NavigatorInterface Fawkes BlackBoard Interface.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
void set_final(const bool new_final)
Set final value.
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Fawkes library namespace.