22 #include "navgraph_adapter.h"
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>
32 #include <AdapterConfiguration.hh>
33 #include <AdapterExecInterface.hh>
34 #include <AdapterFactory.hh>
35 #include <ArrayImpl.hh>
51 : InterfaceAdapter(execInterface)
61 pugi::xml_node
const xml)
62 : InterfaceAdapter(execInterface, xml)
77 logger_ =
reinterpret_cast<fawkes::Logger *
>(m_execInterface.getProperty(
"::Fawkes::Logger"));
79 reinterpret_cast<fawkes::BlackBoard *
>(m_execInterface.getProperty(
"::Fawkes::BlackBoard"));
81 m_execInterface.getProperty(
"::Fawkes::NavGraph"));
90 namespace p = std::placeholders;
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)},
98 for (
const auto &c : commands_) {
99 PLEXIL::g_configuration->registerCommandInterface(c.first,
this);
138 blackboard_->
close(pose_if_);
148 std::string
const &name = cmd->getName();
150 auto c = commands_.find(name);
151 if (c != commands_.end()) {
154 warn(
"NavGraphAdapter:executeCommand: called for unknown"
157 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
158 m_execInterface.notifyOfExternalEvent();
163 NavGraphPlexilAdapter::navgraph_get_nodes(PLEXIL::Command *cmd)
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());
171 m_execInterface.handleCommandReturn(cmd, PLEXIL::Value(array));
172 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_SUCCESS);
173 m_execInterface.notifyOfExternalEvent();
177 NavGraphPlexilAdapter::navgraph_cost_to(PLEXIL::Command *cmd)
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();
188 std::string target_node;
189 args[0].getValue(target_node);
192 warn(
"NavGraphAdapter:navgraph_cost_to:"
193 <<
" Cannot determine distance without pose provider (no writer for" << pose_if_->
uid()
195 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
196 m_execInterface.notifyOfExternalEvent();
209 cost += navgraph_->
cost(src_node, closest);
211 if (closest.
name() != target_node) {
216 warn(
"NavGraphAdapter:navgraph_cost_to:"
217 <<
" Failed to generate path from " << closest.
name() <<
" to " << target_node <<
": "
219 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
220 m_execInterface.notifyOfExternalEvent();
225 m_execInterface.handleCommandReturn(cmd, PLEXIL::Value(cost));
226 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_SUCCESS);
227 m_execInterface.notifyOfExternalEvent();
231 NavGraphPlexilAdapter::navgraph_cost_between(PLEXIL::Command *cmd)
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();
242 std::string source_node;
243 std::string target_node;
244 args[0].getValue(source_node);
245 args[1].getValue(target_node);
255 warn(
"NavGraphAdapter:navgraph_cost_between:"
256 <<
" Failed to generate path from " << source_node <<
" to " << target_node <<
": "
258 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
259 m_execInterface.notifyOfExternalEvent();
263 m_execInterface.handleCommandReturn(cmd, PLEXIL::Value(cost));
264 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_SUCCESS);
265 m_execInterface.notifyOfExternalEvent();
278 initNavGraphAdapter()
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.
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.
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
const char * uid() const
Get unique identifier of interface.
void read()
Read from BlackBoard into local copy.
bool has_writer() const
Check if there is a writer for the interface.
Mutex * objmutex_ptr() const
Get object mutex.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
const std::string & name() const
Get name of node.
Class representing a path for a NavGraph.
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.
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.
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
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.
Position3DInterface Fawkes BlackBoard Interface.
double * translation() const
Get translation value.
Fawkes library namespace.