Skip to content

Commit 1a5a1ad

Browse files
saikishorsaikishor
committed
WIP: Add dependencies finder class
1 parent 37b69b9 commit 1a5a1ad

File tree

2 files changed

+321
-65
lines changed

2 files changed

+321
-65
lines changed

‎controller_manager/include/controller_manager/controller_spec.hpp‎

Lines changed: 280 additions & 51 deletions
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,133 @@ namespace controller_manager
3535
{
3636

3737
using MovingAverageStatistics = ros2_control::MovingAverageStatistics;
38+
39+
struct ControllerPeerInfo
40+
{
41+
std::string name = "";
42+
std::vector<ControllerPeerInfo *> predecessors = {};
43+
std::vector<ControllerPeerInfo *> successors = {};
44+
45+
std::vector<std::string> get_controllers_to_activate() const
46+
{
47+
std::vector<std::string> controllers_to_activate;
48+
for (const auto & successor : successors)
49+
{
50+
ros2_control::add_item(controllers_to_activate, successor->name);
51+
successor->get_successors_and_predecessors(controllers_to_activate, name);
52+
}
53+
ros2_control::remove_item(controllers_to_activate, name);
54+
return controllers_to_activate;
55+
}
56+
57+
std::vector<std::string> get_controllers_to_deactivate() const
58+
{
59+
std::vector<std::string> controllers_to_deactivate;
60+
for (const auto & predecessor : predecessors)
61+
{
62+
ros2_control::add_item(controllers_to_deactivate, predecessor->name);
63+
predecessor->get_predecessors(controllers_to_deactivate, name);
64+
predecessor->get_successors(controllers_to_deactivate, name);
65+
}
66+
ros2_control::remove_item(controllers_to_deactivate, name);
67+
return controllers_to_deactivate;
68+
}
69+
70+
void get_predecessors(std::vector<std::string> &total_list, const std::string &untill_controller) const
71+
{
72+
for (const auto & predecessor : predecessors)
73+
{
74+
if(!predecessor)
75+
{
76+
continue;
77+
}
78+
if(predecessor->name == untill_controller)
79+
{
80+
RCLCPP_INFO(
81+
rclcpp::get_logger("controller_manager"), "skipping predecessor: %s - %s",
82+
predecessor->name.c_str(), untill_controller.c_str());
83+
continue;
84+
}
85+
const std::string predecessor_name = predecessor->name;
86+
if(!ros2_control::has_item(total_list, predecessor_name))
87+
{
88+
RCLCPP_INFO(
89+
rclcpp::get_logger("controller_manager"),
90+
"Getting Predecessor: %s, Successor: %s - %s", predecessor_name.c_str(), name.c_str(), untill_controller.c_str());
91+
total_list.push_back(predecessor_name);
92+
predecessor->get_predecessors(total_list, predecessor_name);
93+
}
94+
}
95+
}
96+
97+
void get_successors(std::vector<std::string> &total_list, const std::string &untill_controller) const
98+
{
99+
for (const auto & successor : successors)
100+
{
101+
if(!successor)
102+
{
103+
continue;
104+
}
105+
if(successor->name == untill_controller)
106+
{
107+
RCLCPP_INFO(
108+
rclcpp::get_logger("controller_manager"), "skipping successor: %s - %s",
109+
successor->name.c_str(), untill_controller.c_str());
110+
continue;
111+
}
112+
const std::string successor_name = successor->name;
113+
if(!ros2_control::has_item(total_list, successor_name))
114+
{
115+
RCLCPP_INFO(
116+
rclcpp::get_logger("controller_manager"),
117+
"Getting Successor: %s, Predecessor: %s - %s", successor_name.c_str(), name.c_str(), untill_controller.c_str());
118+
total_list.push_back(successor_name);
119+
successor->get_successors(total_list, successor_name);
120+
}
121+
}
122+
}
123+
124+
void get_successors_and_predecessors(std::vector<std::string> &total_list, const std::string &untill_controller) const
125+
{
126+
for (const auto & predecessor : predecessors)
127+
{
128+
if(!predecessor)
129+
{
130+
continue;
131+
}
132+
if(predecessor->name == untill_controller)
133+
{
134+
continue;
135+
}
136+
const std::string predecessor_name = predecessor->name;
137+
if(!ros2_control::has_item(total_list, predecessor_name))
138+
{
139+
RCLCPP_INFO(
140+
rclcpp::get_logger("controller_manager"),
141+
"Predecessor: %s, Successor: %s - %s", predecessor_name.c_str(), name.c_str(), untill_controller.c_str());
142+
total_list.push_back(predecessor_name);
143+
predecessor->get_successors_and_predecessors(total_list, predecessor_name);
144+
}
145+
}
146+
for (const auto & successor : successors)
147+
{
148+
if(!successor)
149+
{
150+
continue;
151+
}
152+
if(successor->name == untill_controller)
153+
{
154+
continue;
155+
}
156+
const std::string successor_name = successor->name;
157+
if(!ros2_control::has_item(total_list, successor_name))
158+
{
159+
total_list.push_back(successor_name);
160+
successor->get_successors_and_predecessors(total_list, successor_name);
161+
}
162+
}
163+
}
164+
};
38165
/// Controller Specification
39166
/**
40167
* This struct contains both a pointer to a given controller, \ref c, as well
@@ -68,75 +195,177 @@ class ControllerChainDependencyGraph
68195
public:
69196
void add_dependency(const std::string & predecessor, const std::string & successor)
70197
{
71-
if (predecessors.count(predecessor) == 0)
198+
if(controller_graph_.count(predecessor) == 0)
72199
{
73-
predecessors[predecessor] = {};
200+
controller_graph_[predecessor] = ControllerPeerInfo();
201+
controller_graph_[predecessor].name = predecessor;
74202
}
75-
if (successors.count(successor) == 0)
203+
if(controller_graph_.count(successor) == 0)
76204
{
77-
successors[successor] = {};
205+
controller_graph_[successor] = ControllerPeerInfo();
206+
controller_graph_[successor].name = successor;
78207
}
79-
80-
ros2_control::add_item(predecessors[successor], predecessor);
81-
ros2_control::add_item(successors[predecessor], successor);
208+
controller_graph_[predecessor].successors.push_back(&controller_graph_[successor]);
209+
controller_graph_[successor].predecessors.push_back(&controller_graph_[predecessor]);
82210
}
83211

84-
void remove_controller(const std::string & controller_name)
212+
std::vector<std::string> get_dependencies_to_activate(
213+
const std::string & controller_name)
85214
{
86-
predecessors.erase(controller_name);
87-
successors.erase(controller_name);
88-
for (auto & [_, succ] : predecessors)
89-
{
90-
succ.erase(std::remove(succ.begin(), succ.end(), controller_name), succ.end());
91-
}
92-
for (auto & [_, preds] : successors)
215+
if (controller_graph_.count(controller_name) == 0)
93216
{
94-
preds.erase(std::remove(preds.begin(), preds.end(), controller_name), preds.end());
217+
return {};
95218
}
219+
return controller_graph_[controller_name].get_controllers_to_activate();
96220
}
97221

98-
void depth_first_search(
99-
const std::string & controller_name, std::unordered_set<std::string> & visited,
100-
std::unordered_map<std::string, std::vector<std::string>> & graph)
222+
std::vector<std::string> get_dependencies_to_deactivate(
223+
const std::string & controller_name)
101224
{
102-
if (visited.find(controller_name) != visited.end())
103-
{
104-
return;
105-
}
106-
visited.insert(controller_name);
107-
for (const auto & neighbor : graph[controller_name])
225+
if (controller_graph_.count(controller_name) == 0)
108226
{
109-
if (visited.find(neighbor) == visited.end())
110-
{
111-
depth_first_search(neighbor, visited, graph);
112-
}
227+
return {};
113228
}
114-
}
115-
116-
std::vector<std::string> get_all_predecessors(const std::string & controller_name)
117-
{
118-
std::unordered_set<std::string> visited;
119-
depth_first_search(controller_name, visited, predecessors);
120-
std::vector<std::string> predecessors_list;
121-
std::copy(visited.begin(), visited.end(), std::back_inserter(predecessors_list));
122-
predecessors_list.pop_back(); // Remove the controller itself
123-
return predecessors_list;
124-
}
125-
126-
std::vector<std::string> get_all_successors(const std::string & controller_name)
127-
{
128-
std::unordered_set<std::string> visited;
129-
depth_first_search(controller_name, visited, successors);
130-
std::vector<std::string> successors_list;
131-
std::copy(visited.begin(), visited.end(), std::back_inserter(successors_list));
132-
successors_list.pop_back(); // Remove the controller itself
133-
return successors_list;
229+
return controller_graph_[controller_name].get_controllers_to_deactivate();
134230
}
135231

136232
private:
137-
std::unordered_map<std::string, std::vector<std::string>> predecessors = {};
138-
std::unordered_map<std::string, std::vector<std::string>> successors = {};
233+
std::unordered_map<std::string, ControllerPeerInfo> controller_graph_;
139234
};
140235

236+
237+
// class ControllerChainDependencyGraph
238+
// {
239+
// public:
240+
// void add_dependency(const std::string & predecessor, const std::string & successor)
241+
// {
242+
// if (predecessors.count(predecessor) == 0)
243+
// {
244+
// predecessors[predecessor] = {};
245+
// }
246+
// if (successors.count(successor) == 0)
247+
// {
248+
// successors[successor] = {};
249+
// }
250+
251+
// ros2_control::add_item(predecessors[successor], predecessor);
252+
// ros2_control::add_item(successors[predecessor], successor);
253+
// }
254+
255+
// void remove_controller(const std::string & controller_name)
256+
// {
257+
// predecessors.erase(controller_name);
258+
// successors.erase(controller_name);
259+
// for (auto & [_, succ] : predecessors)
260+
// {
261+
// succ.erase(std::remove(succ.begin(), succ.end(), controller_name), succ.end());
262+
// }
263+
// for (auto & [_, preds] : successors)
264+
// {
265+
// preds.erase(std::remove(preds.begin(), preds.end(), controller_name), preds.end());
266+
// }
267+
// }
268+
269+
// void depth_first_search(
270+
// const std::string & controller_name, std::unordered_set<std::string> & visited,
271+
// std::unordered_map<std::string, std::vector<std::string>> & graph, const std::string & untill_node = "")
272+
// {
273+
// if (visited.find(controller_name) != visited.end())
274+
// {
275+
// return;
276+
// }
277+
// if(!untill_node.empty() && controller_name == untill_node)
278+
// {
279+
// return;
280+
// }
281+
// visited.insert(controller_name);
282+
// for (const auto & neighbor : graph[controller_name])
283+
// {
284+
// if (visited.find(neighbor) == visited.end())
285+
// {
286+
// depth_first_search(neighbor, visited, graph);
287+
// }
288+
// }
289+
// }
290+
291+
// std::vector<std::string> get_all_predecessors(const std::string & controller_name, const std::string & untill_node = "")
292+
// {
293+
// std::unordered_set<std::string> visited;
294+
// depth_first_search(controller_name, visited, predecessors, untill_node);
295+
// std::vector<std::string> predecessors_list;
296+
// std::copy(visited.begin(), visited.end(), std::back_inserter(predecessors_list));
297+
// ros2_control::remove_item(predecessors_list, controller_name);
298+
// return predecessors_list;
299+
// }
300+
301+
// std::vector<std::string> get_all_successors(const std::string & controller_name, const std::string & untill_node = "")
302+
// {
303+
// std::unordered_set<std::string> visited;
304+
// depth_first_search(controller_name, visited, successors, untill_node);
305+
// std::vector<std::string> successors_list;
306+
// std::copy(visited.begin(), visited.end(), std::back_inserter(successors_list));
307+
// ros2_control::remove_item(successors_list, controller_name);
308+
// return successors_list;
309+
// }
310+
311+
// std::vector<std::string> get_dependents_to_activate(const std::string & controller_name)
312+
// {
313+
// std::unordered_set<std::string> visited;
314+
// depth_first_search(controller_name, visited, successors);
315+
// // now for every visited controller look for all it's predecessors and their predecessors and
316+
// // add to the dependents list
317+
// std::vector<std::string> dependents_to_activate;
318+
// std::copy(visited.begin(), visited.end(), std::back_inserter(dependents_to_activate));
319+
// ros2_control::remove_item(dependents_to_activate, controller_name);
320+
// for (const auto & controller : visited)
321+
// {
322+
// std::vector<std::string> predecessors_list = get_all_predecessors(controller, controller_name);
323+
// ros2_control::remove_item(predecessors_list, controller_name);
324+
325+
// for(const auto & predecessor : predecessors_list)
326+
// {
327+
// RCLCPP_INFO(
328+
// rclcpp::get_logger("controller_manager"),
329+
// "Predecessor of %s is %s", controller_name.c_str(), predecessor.c_str());
330+
// std::vector<std::string> successors_of_predecessor = get_all_successors(predecessor, controller_name);
331+
// ros2_control::remove_item(successors_of_predecessor, predecessor);
332+
333+
// for (const auto & succ_pred : successors_of_predecessor)
334+
// {
335+
// RCLCPP_INFO(
336+
// rclcpp::get_logger("controller_manager"),
337+
// "Successor of predecessor %s is %s", predecessor.c_str(), succ_pred.c_str());
338+
// }
339+
// // insert if not already in the list
340+
// std::copy_if(
341+
// successors_of_predecessor.begin(), successors_of_predecessor.end(),
342+
// std::back_inserter(dependents_to_activate),
343+
// [&dependents_to_activate](const std::string & succ_pred)
344+
// {
345+
// return std::find(
346+
// dependents_to_activate.begin(), dependents_to_activate.end(), succ_pred) ==
347+
// dependents_to_activate.end();
348+
// });
349+
// }
350+
// // insert if not already in the list
351+
// std::copy_if(
352+
// predecessors_list.begin(), predecessors_list.end(),
353+
// std::back_inserter(dependents_to_activate),
354+
// [&dependents_to_activate](const std::string & predecessor)
355+
// {
356+
// return std::find(
357+
// dependents_to_activate.begin(), dependents_to_activate.end(), predecessor) ==
358+
// dependents_to_activate.end();
359+
// });
360+
// }
361+
// ros2_control::remove_item(dependents_to_activate, controller_name);
362+
// return dependents_to_activate;
363+
// }
364+
365+
// private:
366+
// std::unordered_map<std::string, std::vector<std::string>> predecessors = {};
367+
// std::unordered_map<std::string, std::vector<std::string>> successors = {};
368+
// };
369+
141370
} // namespace controller_manager
142371
#endif // CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_

0 commit comments

Comments
 (0)