Fawkes API
Fawkes Development Version
visualization_thread.h
1
2
/***************************************************************************
3
* visualization_thread.h - Visualization for pathplan via rviz
4
*
5
* Created: Fri Nov 11 21:17:24 2011
6
* Copyright 2011-2012 Tim Niemueller [www.niemueller.de]
7
****************************************************************************/
8
9
/* This program is free software; you can redistribute it and/or modify
10
* it under the terms of the GNU General Public License as published by
11
* the Free Software Foundation; either version 2 of the License, or
12
* (at your option) any later version.
13
*
14
* This program is distributed in the hope that it will be useful,
15
* but WITHOUT ANY WARRANTY; without even the implied warranty of
16
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17
* GNU Library General Public License for more details.
18
*
19
* Read the full text in the LICENSE.GPL file in the doc directory.
20
*/
21
22
#ifndef _PLUGINS_VISPATHPLAN_VISPATHPLAN_THREAD_H_
23
#define _PLUGINS_VISPATHPLAN_VISPATHPLAN_THREAD_H_
24
25
#include <aspect/configurable.h>
26
#include <aspect/logging.h>
27
#include <core/threading/mutex.h>
28
#include <core/threading/thread.h>
29
#include <core/utils/lockptr.h>
30
#include <navgraph/navgraph.h>
31
#include <navgraph/navgraph_path.h>
32
#include <plugins/ros/aspect/ros.h>
33
#include <ros/publisher.h>
34
#include <visualization_msgs/MarkerArray.h>
35
36
class
NavGraphVisualizationThread
:
public
fawkes::Thread
,
37
public
fawkes::ConfigurableAspect
,
38
public
fawkes::LoggingAspect
,
39
public
fawkes::ROSAspect
,
40
public
fawkes::NavGraph::ChangeListener
41
{
42
public
:
43
NavGraphVisualizationThread
();
44
45
virtual
void
init
();
46
virtual
void
loop
();
47
virtual
void
finalize
();
48
49
void
set_graph
(
fawkes::LockPtr<fawkes::NavGraph>
&graph);
50
void
set_constraint_repo
(
fawkes::LockPtr<fawkes::NavGraphConstraintRepo>
&crepo);
51
52
void
set_traversal
(
fawkes::NavGraphPath::Traversal
&traversal);
53
void
set_current_edge
(
const
std::string &from,
const
std::string &to);
54
void
reset_plan
();
55
56
virtual
void
graph_changed
()
throw
();
57
58
private
:
59
void
publish();
60
void
add_circle_markers(visualization_msgs::MarkerArray &m,
61
size_t
& id_num,
62
float
center_x,
63
float
center_y,
64
float
radius,
65
unsigned
int
arc_length,
66
float
r,
67
float
g,
68
float
b,
69
float
alpha,
70
float
line_width = 0.03);
71
float
edge_cost_factor(std::list<std::tuple<std::string, std::string, std::string, float>> &costs,
72
const
std::string & from,
73
const
std::string & to,
74
std::string &constraint_name);
75
76
private
:
77
size_t
last_id_num_;
78
size_t
constraints_last_id_num_;
79
ros::Publisher vispub_;
80
81
float
cfg_cost_scale_max_;
82
83
fawkes::NavGraphPath::Traversal
traversal_;
84
std::string plan_to_;
85
std::string plan_from_;
86
87
fawkes::LockPtr<fawkes::NavGraph>
graph_;
88
fawkes::LockPtr<fawkes::NavGraphConstraintRepo>
crepo_;
89
90
std::string cfg_global_frame_;
91
};
92
93
#endif
fawkes::LockPtr< fawkes::NavGraph >
NavGraphVisualizationThread::reset_plan
void reset_plan()
Reset the current plan.
Definition:
visualization_thread.cpp:145
fawkes::NavGraph::ChangeListener
Definition:
navgraph.h:183
NavGraphVisualizationThread::loop
virtual void loop()
Code to execute in the thread.
Definition:
visualization_thread.cpp:173
NavGraphVisualizationThread::init
virtual void init()
Initialize the thread.
Definition:
visualization_thread.cpp:52
NavGraphVisualizationThread::set_graph
void set_graph(fawkes::LockPtr< fawkes::NavGraph > &graph)
Set the graph.
Definition:
visualization_thread.cpp:115
NavGraphVisualizationThread
Definition:
visualization_thread.h:36
NavGraphVisualizationThread::graph_changed
virtual void graph_changed()
Definition:
visualization_thread.cpp:167
fawkes::LoggingAspect
Definition:
logging.h:38
NavGraphVisualizationThread::finalize
virtual void finalize()
Finalize the thread.
Definition:
visualization_thread.cpp:74
NavGraphVisualizationThread::set_current_edge
void set_current_edge(const std::string &from, const std::string &to)
Set the currently executed edge of the plan.
Definition:
visualization_thread.cpp:157
fawkes::ROSAspect
Definition:
ros.h:38
NavGraphVisualizationThread::set_traversal
void set_traversal(fawkes::NavGraphPath::Traversal &traversal)
Set current path.
Definition:
visualization_thread.cpp:136
fawkes::Thread
Definition:
thread.h:45
fawkes::ConfigurableAspect
Definition:
configurable.h:38
fawkes::NavGraphPath::Traversal
Definition:
navgraph_path.h:44
NavGraphVisualizationThread::NavGraphVisualizationThread
NavGraphVisualizationThread()
Constructor.
Definition:
visualization_thread.cpp:43
NavGraphVisualizationThread::set_constraint_repo
void set_constraint_repo(fawkes::LockPtr< fawkes::NavGraphConstraintRepo > &crepo)
Set the constraint repo.
Definition:
visualization_thread.cpp:127
src
plugins
navgraph
visualization_thread.h
Generated by
1.8.17