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 
23 namespace 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 
47 bool
49 {
50  const std::string dest_name = get_property(dest_names_);
51  return navgraph_->node_exists(skill.skill_args.at(dest_name));
52 }
53 
54 float
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 
72 std::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.