Fawkes API Fawkes Development Version
navgraph_clusters_thread.h
1/***************************************************************************
2 * navgraph_clusters_thread.h - block paths based on laser clusters
3 *
4 * Created: Sun Jul 13 15:30:03 2014
5 * Copyright 2014 Tim Niemueller [www.niemueller.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#ifndef _PLUGINS_NAVGRAPH_CLUSTERS_NAVGRAPH_CLUSTERS_THREAD_H_
22#define _PLUGINS_NAVGRAPH_CLUSTERS_NAVGRAPH_CLUSTERS_THREAD_H_
23
24#include <aspect/blackboard.h>
25#include <aspect/clock.h>
26#include <aspect/configurable.h>
27#include <aspect/logging.h>
28#include <aspect/tf.h>
29#include <blackboard/interface_listener.h>
30#include <blackboard/interface_observer.h>
31#include <core/threading/thread.h>
32#include <core/utils/lock_list.h>
33#include <navgraph/aspect/navgraph.h>
34
35#include <Eigen/Geometry>
36#include <list>
37#include <string>
38#include <tuple>
39
40namespace fawkes {
41class Position3DInterface;
42class Time;
43class NavGraphEdgeConstraint;
44class NavGraphEdgeCostConstraint;
45} // namespace fawkes
46
56{
57public:
60
61 virtual void init();
62 virtual void loop();
63 virtual void finalize();
64
65 std::list<std::pair<std::string, std::string>> blocked_edges() noexcept;
66
67 std::list<std::tuple<std::string, std::string, Eigen::Vector2f>>
68 blocked_edges_centroids() noexcept;
69
70 bool robot_pose(Eigen::Vector2f &pose) noexcept;
71
72 /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
73protected:
74 virtual void
76 {
77 Thread::run();
78 }
79
80private:
81 // for BlackBoardInterfaceObserver
82 virtual void bb_interface_created(const char *type, const char *id) noexcept;
83
84 // for BlackBoardInterfaceListener
85 virtual void bb_interface_writer_removed(fawkes::Interface *interface,
86 fawkes::Uuid instance_serial) noexcept;
87 virtual void bb_interface_reader_removed(fawkes::Interface *interface,
88 fawkes::Uuid instance_serial) noexcept;
89
90 void conditional_close(fawkes::Interface *interface) noexcept;
91
92 Eigen::Vector2f
93 fixed_frame_pose(std::string frame, const fawkes::Time &timestamp, float x, float y);
94
95private:
96 std::string cfg_iface_prefix_;
97 float cfg_close_threshold_;
98 std::string cfg_fixed_frame_;
99 std::string cfg_base_frame_;
100 int cfg_min_vishistory_;
101 std::string cfg_mode_;
102
104
105 fawkes::NavGraphEdgeConstraint * edge_constraint_;
106 fawkes::NavGraphEdgeCostConstraint *edge_cost_constraint_;
107};
108
109#endif
Block navgraph paths based on laser clusters.
std::list< std::tuple< std::string, std::string, Eigen::Vector2f > > blocked_edges_centroids() noexcept
Get a list of edges close to a clusters and its centroid considered blocked.
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
bool robot_pose(Eigen::Vector2f &pose) noexcept
Determine current robot pose.
std::list< std::pair< std::string, std::string > > blocked_edges() noexcept
Get a list of edges close to a clusters considered blocked.
virtual void run()
Stub to see name in backtrace for easier debugging.
virtual ~NavGraphClustersThread()
Destructor.
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
BlackBoard interface listener.
BlackBoard interface observer.
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:34
Thread aspect to access configuration data.
Definition: configurable.h:33
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
Thread aspect to log output.
Definition: logging.h:33
Thread aspect to access NavGraph.
Definition: navgraph.h:36
Constraint that can be queried to check if an edge is blocked.
Constraint that can be queried for an edge cost factor.
Thread class encapsulation of pthreads.
Definition: thread.h:46
A class for handling time.
Definition: time.h:93
Thread aspect to access the transform system.
Definition: tf.h:39
A convenience class for universally unique identifiers (UUIDs).
Definition: uuid.h:29
Fawkes library namespace.