-
Notifications
You must be signed in to change notification settings - Fork 15
/
plugin_container.cpp
120 lines (88 loc) · 3.05 KB
/
plugin_container.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
#include "ed/plugin_container.h"
#include "ed/plugin.h"
// TODO: get rid of ros rate
#include <ros/rate.h>
namespace ed
{
// --------------------------------------------------------------------------------
PluginContainer::PluginContainer()
: cycle_duration_(0.1), loop_frequency_(10), stop_(false), step_finished_(true), t_last_update_(0)
{
timer_.start();
}
// --------------------------------------------------------------------------------
PluginContainer::~PluginContainer()
{
}
// --------------------------------------------------------------------------------
void PluginContainer::setPlugin(PluginPtr plugin, const std::string& name)
{
plugin_ = plugin;
name_ = name;
plugin_->name_ = name;
}
// --------------------------------------------------------------------------------
//UpdateRequestConstPtr PluginContainer::getAndClearUpdateRequest() {
// UpdateRequestConstPtr ret = update_request_;
// update_request_ = UpdateRequestPtr(new UpdateRequest());
// return ret;
//}
// --------------------------------------------------------------------------------
void PluginContainer::runThreaded()
{
thread_ = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&PluginContainer::run, this)));
}
// --------------------------------------------------------------------------------
void PluginContainer::run()
{
ros::Rate r(loop_frequency_);
while(!stop_)
{
step();
r.sleep();
}
}
// --------------------------------------------------------------------------------
void PluginContainer::step()
{
// If we still have an update_request, it means the request is not yet handled,
// so we have to skip this cycle (and wait until the world model has handled it)
if (update_request_)
return;
// Check if there is a new world. If so replace the current one with the new one
if (world_new_)
{
// TODO: add mutex
world_current_ = world_new_;
world_new_.reset();
}
if (world_current_)
{
UpdateRequestPtr update_request(new UpdateRequest);
plugin_->process(*world_current_, *update_request);
// If the received update_request was not empty, set it
if (!update_request->empty())
update_request_ = update_request;
}
}
// --------------------------------------------------------------------------------
void PluginContainer::threadedStep(const WorldModelConstPtr& world)
{
double time = timer_.getElapsedTimeInSec();
double secs_since_last_update = time - t_last_update_;
if (secs_since_last_update < cycle_duration_)
return;
step_finished_ = false;
t_last_update_ = time;
world_new_ = world;
thread_ = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&PluginContainer::step, this)));
}
// --------------------------------------------------------------------------------
void PluginContainer::stop()
{
stop_ = true;
thread_->join();
plugin_.reset();
}
// --------------------------------------------------------------------------------
}