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 initialize()
Initialize adapter.
Fawkes library namespace.
void invokeAbort(PLEXIL::Command *cmd)
Abort currently running execution.
virtual bool stop()
Stop adapter.
Class representing a path for a NavGraph.
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.
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
virtual ~NavGraphPlexilAdapter()
Destructor.
float cost() const
Get cost of path from start to to end.
Position3DInterface Fawkes BlackBoard Interface.
Base class for exceptions in Fawkes.
void read()
Read from BlackBoard into local copy.
Mutex * objmutex_ptr() const
Get object mutex.
const std::string & name() const
Get name of node.
bool has_writer() const
Check if there is a writer for the interface.
const char * uid() const
Get unique identifier of interface.
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
virtual bool reset()
Reset adapter.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
double * translation() const
Get translation value.
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.
void executeCommand(PLEXIL::Command *cmd)
Perform given command.
virtual bool start()
Start adapter.
NavGraphPlexilAdapter(PLEXIL::AdapterExecInterface &execInterface)
Constructor.
float cost(const NavGraphNode &from, const NavGraphNode &to) const
Calculate cost between two adjacent nodes.
virtual void close(Interface *interface)=0
Close interface.
virtual bool shutdown()
Shut adapter down.