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 
40 using 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  */
50 NavGraphPlexilAdapter::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  */
60 NavGraphPlexilAdapter::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  */
74 bool
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  */
108 bool
110 {
111  return true;
112 }
113 
114 /** Stop adapter.
115  * @return true if successful, false otherwise.
116  */
117 bool
119 {
120  return true;
121 }
122 
123 /** Reset adapter.
124  * @return true if successful, false otherwise.
125  */
126 bool
128 {
129  return true;
130 }
131 
132 /** Shut adapter down.
133  * @return true if successful, false otherwise.
134  */
135 bool
137 {
138  blackboard_->close(pose_if_);
139  return true;
140 }
141 
142 /** Perform given command.
143  * @param cmd command to execute
144  */
145 void
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 
162 void
163 NavGraphPlexilAdapter::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 
176 void
177 NavGraphPlexilAdapter::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 
230 void
231 NavGraphPlexilAdapter::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  */
271 void
273 {
274 }
275 
276 extern "C" {
277 void
278 initNavGraphAdapter()
279 {
280  REGISTER_ADAPTER(NavGraphPlexilAdapter, "NavGraphAdapter");
281 }
282 }
fawkes::NavGraphNode
Definition: navgraph_node.h:40
NavGraphPlexilAdapter::start
virtual bool start()
Start adapter.
Definition: navgraph_adapter.cpp:109
fawkes::LockPtr< fawkes::NavGraph >
NavGraphPlexilAdapter
Interface adapter to provide logging facilities.
Definition: navgraph_adapter.h:41
NavGraphPlexilAdapter::executeCommand
void executeCommand(PLEXIL::Command *cmd)
Perform given command.
Definition: navgraph_adapter.cpp:146
fawkes::Interface::read
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:477
NavGraphPlexilAdapter::reset
virtual bool reset()
Reset adapter.
Definition: navgraph_adapter.cpp:127
fawkes::NavGraph::closest_node
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:191
fawkes::MutexLocker
Definition: mutex_locker.h:39
fawkes::LockPtr::objmutex_ptr
Mutex * objmutex_ptr() const
Get object mutex.
Definition: lockptr.h:300
fawkes::BlackBoard
Definition: blackboard.h:50
NavGraphPlexilAdapter::initialize
virtual bool initialize()
Initialize adapter.
Definition: navgraph_adapter.cpp:75
NavGraphPlexilAdapter::invokeAbort
void invokeAbort(PLEXIL::Command *cmd)
Abort currently running execution.
Definition: navgraph_adapter.cpp:272
fawkes::NavGraphNode::name
const std::string & name() const
Get name of node.
Definition: navgraph_node.h:60
fawkes::Position3DInterface::translation
double * translation() const
Get translation value.
Definition: Position3DInterface.cpp:228
NavGraphPlexilAdapter::~NavGraphPlexilAdapter
virtual ~NavGraphPlexilAdapter()
Destructor.
Definition: navgraph_adapter.cpp:67
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
fawkes::Logger::log_error
virtual void log_error(const char *component, const char *format,...)=0
fawkes::Logger
Definition: logger.h:41
fawkes::NavGraphPath::cost
float cost() const
Get cost of path from start to to end.
Definition: navgraph_path.h:138
fawkes
fawkes::NavGraph::cost
float cost(const NavGraphNode &from, const NavGraphNode &to) const
Calculate cost between two adjacent nodes.
Definition: navgraph.cpp:1128
NavGraphPlexilAdapter::NavGraphPlexilAdapter
NavGraphPlexilAdapter(PLEXIL::AdapterExecInterface &execInterface)
Constructor.
Definition: navgraph_adapter.cpp:50
fawkes::Interface::has_writer
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:819
fawkes::Interface::uid
const char * uid() const
Get unique identifier of interface.
Definition: interface.cpp:679
fawkes::Exception::what_no_backtrace
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
fawkes::NavGraph::search_path
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:1005
fawkes::Position3DInterface
Definition: Position3DInterface.h:39
fawkes::NavGraph::nodes
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
Definition: navgraph.cpp:138
fawkes::BlackBoard::open_for_reading
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
fawkes::NavGraphPath
Definition: navgraph_path.h:41
NavGraphPlexilAdapter::shutdown
virtual bool shutdown()
Shut adapter down.
Definition: navgraph_adapter.cpp:136
NavGraphPlexilAdapter::stop
virtual bool stop()
Stop adapter.
Definition: navgraph_adapter.cpp:118
fawkes::Exception
Definition: exception.h:41