Fawkes API Fawkes Development Version
navgraph_adapter.cpp
1
2/***************************************************************************
3 * navgraph_adapter.cpp - PLEXIL adapter for the Behavior Engine
4 *
5 * Created: Sun Aug 19 20:57:53 2018
6 * Copyright 2006-2018 Tim Niemueller [www.niemueller.de]
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_adapter.h"
23
24#include "utils.h"
25
26#include <blackboard/blackboard.h>
27#include <core/threading/mutex_locker.h>
28#include <interfaces/Position3DInterface.h>
29#include <logging/logger.h>
30#include <navgraph/navgraph.h>
31
32#include <AdapterConfiguration.hh>
33#include <AdapterExecInterface.hh>
34#include <AdapterFactory.hh>
35#include <ArrayImpl.hh>
36#include <Command.hh>
37#include <Error.hh>
38#include <algorithm>
39
40using namespace fawkes;
41
42/** @class NavGraphPlexilAdapter "navgraph_adapter.h"
43 * Plexil adapter to provide access to the NavGraph.
44 * @author Tim Niemueller
45 */
46
47/** Constructor.
48 * @param execInterface Reference to the parent AdapterExecInterface object.
49 */
50NavGraphPlexilAdapter::NavGraphPlexilAdapter(PLEXIL::AdapterExecInterface &execInterface)
51: InterfaceAdapter(execInterface)
52{
53}
54
55/** Constructor from configuration XML.
56 * @param execInterface Reference to the parent AdapterExecInterface object.
57 * @param xml A const reference to the XML element describing this adapter
58 * @note The instance maintains a shared pointer to the XML.
59 */
60NavGraphPlexilAdapter::NavGraphPlexilAdapter(PLEXIL::AdapterExecInterface &execInterface,
61 pugi::xml_node const xml)
62: InterfaceAdapter(execInterface, xml)
63{
64}
65
66/** Destructor. */
68{
69}
70
71/** Initialize adapter.
72 * @return true if initialization was successful, false otherwise.
73 */
74bool
76{
77 logger_ = reinterpret_cast<fawkes::Logger *>(m_execInterface.getProperty("::Fawkes::Logger"));
78 blackboard_ =
79 reinterpret_cast<fawkes::BlackBoard *>(m_execInterface.getProperty("::Fawkes::BlackBoard"));
80 navgraph_ = *reinterpret_cast<fawkes::LockPtr<fawkes::NavGraph> *>(
81 m_execInterface.getProperty("::Fawkes::NavGraph"));
82
83 try {
84 pose_if_ = blackboard_->open_for_reading<Position3DInterface>("Pose");
85 } catch (Exception &e) {
86 logger_->log_error("PlexilBE", "Failed to open pose interface: %s", e.what_no_backtrace());
87 return false;
88 }
89
90 namespace p = std::placeholders;
91 commands_ = {
92 {"navgraph_get_nodes", std::bind(&NavGraphPlexilAdapter::navgraph_get_nodes, this, p::_1)},
93 {"navgraph_cost_to", std::bind(&NavGraphPlexilAdapter::navgraph_cost_to, this, p::_1)},
94 {"navgraph_cost_between",
95 std::bind(&NavGraphPlexilAdapter::navgraph_cost_between, this, p::_1)},
96 };
97
98 for (const auto &c : commands_) {
99 PLEXIL::g_configuration->registerCommandInterface(c.first, this);
100 }
101
102 return true;
103}
104
105/** Start adapter.
106 * @return true if starting was successful, false otherwise.
107 */
108bool
110{
111 return true;
112}
113
114/** Stop adapter.
115 * @return true if successful, false otherwise.
116 */
117bool
119{
120 return true;
121}
122
123/** Reset adapter.
124 * @return true if successful, false otherwise.
125 */
126bool
128{
129 return true;
130}
131
132/** Shut adapter down.
133 * @return true if successful, false otherwise.
134 */
135bool
137{
138 blackboard_->close(pose_if_);
139 return true;
140}
141
142/** Perform given command.
143 * @param cmd command to execute
144 */
145void
147{
148 std::string const &name = cmd->getName();
149
150 auto c = commands_.find(name);
151 if (c != commands_.end()) {
152 c->second(cmd);
153 } else {
154 warn("NavGraphAdapter:executeCommand: called for unknown"
155 " command "
156 << name);
157 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
158 m_execInterface.notifyOfExternalEvent();
159 }
160}
161
162void
163NavGraphPlexilAdapter::navgraph_get_nodes(PLEXIL::Command *cmd)
164{
165 fawkes::MutexLocker lock(navgraph_.objmutex_ptr());
166 const std::vector<NavGraphNode> &nodes = navgraph_->nodes();
167 PLEXIL::StringArray array(nodes.size());
168 for (size_t i = 0; i < nodes.size(); ++i) {
169 array.setElement(i, nodes[i].name());
170 }
171 m_execInterface.handleCommandReturn(cmd, PLEXIL::Value(array));
172 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_SUCCESS);
173 m_execInterface.notifyOfExternalEvent();
174}
175
176void
177NavGraphPlexilAdapter::navgraph_cost_to(PLEXIL::Command *cmd)
178{
179 std::vector<PLEXIL::Value> const &args = cmd->getArgValues();
180 if (!verify_args(args,
181 "NavGraphAdapter:navgraph_cost_to",
182 {{"target_node", PLEXIL::STRING_TYPE}})) {
183 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
184 m_execInterface.notifyOfExternalEvent();
185 return;
186 }
187
188 std::string target_node;
189 args[0].getValue(target_node);
190
191 if (!pose_if_->has_writer()) {
192 warn("NavGraphAdapter:navgraph_cost_to:"
193 << " Cannot determine distance without pose provider (no writer for" << pose_if_->uid()
194 << ")");
195 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
196 m_execInterface.notifyOfExternalEvent();
197 return;
198 }
199
200 fawkes::MutexLocker lock(navgraph_.objmutex_ptr());
201
202 pose_if_->read();
203
204 float cost = 0;
205
206 NavGraphNode closest =
207 navgraph_->closest_node(pose_if_->translation(0), pose_if_->translation(1));
208 NavGraphNode src_node("Pose", pose_if_->translation(0), pose_if_->translation(1));
209 cost += navgraph_->cost(src_node, closest);
210
211 if (closest.name() != target_node) {
212 try {
213 NavGraphPath path = navgraph_->search_path(closest.name(), target_node);
214 cost += path.cost();
215 } catch (Exception &e) {
216 warn("NavGraphAdapter:navgraph_cost_to:"
217 << " Failed to generate path from " << closest.name() << " to " << target_node << ": "
218 << e.what_no_backtrace());
219 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
220 m_execInterface.notifyOfExternalEvent();
221 return;
222 }
223 }
224
225 m_execInterface.handleCommandReturn(cmd, PLEXIL::Value(cost));
226 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_SUCCESS);
227 m_execInterface.notifyOfExternalEvent();
228}
229
230void
231NavGraphPlexilAdapter::navgraph_cost_between(PLEXIL::Command *cmd)
232{
233 std::vector<PLEXIL::Value> const &args = cmd->getArgValues();
234 if (!verify_args(args,
235 "NavGraphAdapter:navgraph_cost_to",
236 {{"source_node", PLEXIL::STRING_TYPE}, {"target_node", PLEXIL::STRING_TYPE}})) {
237 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
238 m_execInterface.notifyOfExternalEvent();
239 return;
240 }
241
242 std::string source_node;
243 std::string target_node;
244 args[0].getValue(source_node);
245 args[1].getValue(target_node);
246
247 fawkes::MutexLocker lock(navgraph_.objmutex_ptr());
248
249 float cost = 0.;
250
251 try {
252 NavGraphPath path = navgraph_->search_path(source_node, target_node);
253 cost = path.cost();
254 } catch (Exception &e) {
255 warn("NavGraphAdapter:navgraph_cost_between:"
256 << " Failed to generate path from " << source_node << " to " << target_node << ": "
257 << e.what_no_backtrace());
258 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
259 m_execInterface.notifyOfExternalEvent();
260 return;
261 }
262
263 m_execInterface.handleCommandReturn(cmd, PLEXIL::Value(cost));
264 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_SUCCESS);
265 m_execInterface.notifyOfExternalEvent();
266}
267
268/** Abort currently running execution.
269 * @param cmd command to abort
270 */
271void
273{
274}
275
276extern "C" {
277void
278initNavGraphAdapter()
279{
280 REGISTER_ADAPTER(NavGraphPlexilAdapter, "NavGraphAdapter");
281}
282}
Interface adapter to provide logging facilities.
virtual bool shutdown()
Shut adapter down.
virtual bool start()
Start adapter.
NavGraphPlexilAdapter(PLEXIL::AdapterExecInterface &execInterface)
Constructor.
void invokeAbort(PLEXIL::Command *cmd)
Abort currently running execution.
virtual bool reset()
Reset adapter.
virtual bool initialize()
Initialize adapter.
void executeCommand(PLEXIL::Command *cmd)
Perform given command.
virtual bool stop()
Stop adapter.
virtual ~NavGraphPlexilAdapter()
Destructor.
The BlackBoard abstract class.
Definition: blackboard.h:46
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual void close(Interface *interface)=0
Close interface.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
const char * uid() const
Get unique identifier of interface.
Definition: interface.cpp:686
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:479
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:848
Mutex * objmutex_ptr() const
Get object mutex.
Definition: lockptr.h:284
Interface for logging.
Definition: logger.h:42
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
Mutex locking helper.
Definition: mutex_locker.h:34
Topological graph node.
Definition: navgraph_node.h:36
const std::string & name() const
Get name of node.
Definition: navgraph_node.h:50
Class representing a path for a NavGraph.
Definition: navgraph_path.h:37
float cost() const
Get cost of path from start to to end.
float cost(const NavGraphNode &from, const NavGraphNode &to) const
Calculate cost between two adjacent nodes.
Definition: navgraph.cpp:1123
NavGraphNode closest_node(float pos_x, float pos_y, const std::string &property="") const
Get node closest to a specified point with a certain property.
Definition: navgraph.cpp:186
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
Definition: navgraph.cpp:133
fawkes::NavGraphPath search_path(const std::string &from, const std::string &to, bool use_constraints=true, bool compute_constraints=true)
Search for a path between two nodes with default distance costs.
Definition: navgraph.cpp:1000
Position3DInterface Fawkes BlackBoard Interface.
double * translation() const
Get translation value.
Fawkes library namespace.