Fawkes API Fawkes Development Version
navgraph_estimator.cpp
1/***************************************************************************
2 * navgraph_estimator.cpp - Estimate skill exec times from the navgraph
3 *
4 * Created: Tue 07 Jan 2020 16:35:31 CET 16:35
5 * Copyright 2020 Till Hofmann <hofmann@kbsg.rwth-aachen.de>
6 ****************************************************************************/
7
8/* This program is free software; you can redistribute it and/or modify
9 * it under the terms of the GNU General Public License as published by
10 * the Free Software Foundation; either version 2 of the License, or
11 * (at your option) any later version.
12 *
13 * This program is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 * GNU Library General Public License for more details.
17 *
18 * Read the full text in the LICENSE.GPL file in the doc directory.
19 */
20
21#include "navgraph_estimator.h"
22
23namespace fawkes {
24
25/** @class NavGraphEstimator
26 * Estimate the execution time for the skill goto by querying the distance from
27 * the navgraph.
28 */
29
30/** Constructor.
31 * @param navgraph The navgraph to read the node positions from
32 * @param config The config to read the initial position from
33 * @param cfg_prefix The config prefix to use for config parameters
34 */
36 Configuration * config,
37 const std::string &cfg_prefix)
38: ExecutionTimeEstimator(config, cfg_prefix),
39 navgraph_(navgraph),
40 source_names_(config_, cfg_prefix_, "start", ""),
41 dest_names_(config_, cfg_prefix_, "target")
42{
43 last_pose_x_ = config->get_float_or_default("plugins/amcl/init_pose_x", 0);
44 last_pose_y_ = config->get_float_or_default("plugins/amcl/init_pose_y", 0);
45}
46
47bool
49{
50 const std::string dest_name = get_property(dest_names_);
51 return navgraph_->node_exists(skill.skill_args.at(dest_name));
52}
53
54float
56{
57 float current_pose_x = last_pose_x_;
58 float current_pose_y = last_pose_y_;
59 std::string source_name = get_property(source_names_);
60 if (source_name != "") {
61 const std::string start = skill.skill_args.at(source_name);
62 if (navgraph_->node_exists(start)) {
63 current_pose_x = navgraph_->node(start).x();
64 current_pose_y = navgraph_->node(start).y();
65 }
66 }
67 return navgraph_->node(skill.skill_args.at(get_property(dest_names_)))
68 .distance(current_pose_x, current_pose_y)
69 / speed_;
70}
71
72std::pair<SkillerInterface::SkillStatusEnum, std::string>
74{
75 auto node = navgraph_->node(skill.skill_args.at("place"));
76 last_pose_x_ = node.x();
77 last_pose_y_ = node.y();
78 return std::make_pair(SkillerInterface::SkillStatusEnum::S_FINAL, "");
79}
80
81} // namespace fawkes
Interface for configuration handling.
Definition: config.h:68
virtual float get_float_or_default(const char *path, const float &default_val)
Get value from configuration which is of type float, or the given default if the path does not exist.
Definition: config.cpp:696
A structured representation of a skill.
std::unordered_map< std::string, std::string > skill_args
A map of the skill's argument keys to argument values.
An abstract estimator for the execution time of a skill.
T get_property(const Property< T > &property) const
Get the current property value for active_whitelist_entry_.
const float speed_
Config estimator-specific speedup factor.
LockPtr<> is a reference-counting shared lockable smartpointer.
Definition: lockptr.h:55
bool can_provide_exec_time(const Skill &skill) const override
Check if this estimator can give an estimate for a given skill.
float get_execution_time(const Skill &skill) override
Get the estimated execution time for the given skill string.
std::pair< SkillerInterface::SkillStatusEnum, std::string > execute(const Skill &skill) override
Let the estimator know that we are executing this skill, so it can apply possible side effects.
NavGraphEstimator(LockPtr< NavGraph > navgraph, Configuration *config, const std::string &cfg_prefix)
Constructor.
Fawkes library namespace.