@@ -612,141 +612,5 @@ class ControllerChainDependencyGraph
612
612
std::unordered_map<std::string, ControllerPeerInfo> controller_graph_;
613
613
};
614
614
615
- // class ControllerChainDependencyGraph
616
- // {
617
- // public:
618
- // void add_dependency(const std::string & predecessor, const std::string & successor)
619
- // {
620
- // if (predecessors.count(predecessor) == 0)
621
- // {
622
- // predecessors[predecessor] = {};
623
- // }
624
- // if (successors.count(successor) == 0)
625
- // {
626
- // successors[successor] = {};
627
- // }
628
-
629
- // ros2_control::add_item(predecessors[successor], predecessor);
630
- // ros2_control::add_item(successors[predecessor], successor);
631
- // }
632
-
633
- // void remove_controller(const std::string & controller_name)
634
- // {
635
- // predecessors.erase(controller_name);
636
- // successors.erase(controller_name);
637
- // for (auto & [_, succ] : predecessors)
638
- // {
639
- // succ.erase(std::remove(succ.begin(), succ.end(), controller_name), succ.end());
640
- // }
641
- // for (auto & [_, preds] : successors)
642
- // {
643
- // preds.erase(std::remove(preds.begin(), preds.end(), controller_name), preds.end());
644
- // }
645
- // }
646
-
647
- // void depth_first_search(
648
- // const std::string & controller_name, std::unordered_set<std::string> & visited,
649
- // std::unordered_map<std::string, std::vector<std::string>> & graph, const std::string &
650
- // untill_node = "")
651
- // {
652
- // if (visited.find(controller_name) != visited.end())
653
- // {
654
- // return;
655
- // }
656
- // if(!untill_node.empty() && controller_name == untill_node)
657
- // {
658
- // return;
659
- // }
660
- // visited.insert(controller_name);
661
- // for (const auto & neighbor : graph[controller_name])
662
- // {
663
- // if (visited.find(neighbor) == visited.end())
664
- // {
665
- // depth_first_search(neighbor, visited, graph);
666
- // }
667
- // }
668
- // }
669
-
670
- // std::vector<std::string> get_all_predecessors(const std::string & controller_name, const
671
- // std::string & untill_node = "")
672
- // {
673
- // std::unordered_set<std::string> visited;
674
- // depth_first_search(controller_name, visited, predecessors, untill_node);
675
- // std::vector<std::string> predecessors_list;
676
- // std::copy(visited.begin(), visited.end(), std::back_inserter(predecessors_list));
677
- // ros2_control::remove_item(predecessors_list, controller_name);
678
- // return predecessors_list;
679
- // }
680
-
681
- // std::vector<std::string> get_all_successors(const std::string & controller_name, const
682
- // std::string & untill_node = "")
683
- // {
684
- // std::unordered_set<std::string> visited;
685
- // depth_first_search(controller_name, visited, successors, untill_node);
686
- // std::vector<std::string> successors_list;
687
- // std::copy(visited.begin(), visited.end(), std::back_inserter(successors_list));
688
- // ros2_control::remove_item(successors_list, controller_name);
689
- // return successors_list;
690
- // }
691
-
692
- // std::vector<std::string> get_dependents_to_activate(const std::string & controller_name)
693
- // {
694
- // std::unordered_set<std::string> visited;
695
- // depth_first_search(controller_name, visited, successors);
696
- // // now for every visited controller look for all it's predecessors and their predecessors and
697
- // // add to the dependents list
698
- // std::vector<std::string> dependents_to_activate;
699
- // std::copy(visited.begin(), visited.end(), std::back_inserter(dependents_to_activate));
700
- // ros2_control::remove_item(dependents_to_activate, controller_name);
701
- // for (const auto & controller : visited)
702
- // {
703
- // std::vector<std::string> predecessors_list = get_all_predecessors(controller,
704
- // controller_name); ros2_control::remove_item(predecessors_list, controller_name);
705
-
706
- // for(const auto & predecessor : predecessors_list)
707
- // {
708
- // RCLCPP_INFO(
709
- // rclcpp::get_logger("controller_manager"),
710
- // "Predecessor of %s is %s", controller_name.c_str(), predecessor.c_str());
711
- // std::vector<std::string> successors_of_predecessor = get_all_successors(predecessor,
712
- // controller_name); ros2_control::remove_item(successors_of_predecessor, predecessor);
713
-
714
- // for (const auto & succ_pred : successors_of_predecessor)
715
- // {
716
- // RCLCPP_INFO(
717
- // rclcpp::get_logger("controller_manager"),
718
- // "Successor of predecessor %s is %s", predecessor.c_str(), succ_pred.c_str());
719
- // }
720
- // // insert if not already in the list
721
- // std::copy_if(
722
- // successors_of_predecessor.begin(), successors_of_predecessor.end(),
723
- // std::back_inserter(dependents_to_activate),
724
- // [&dependents_to_activate](const std::string & succ_pred)
725
- // {
726
- // return std::find(
727
- // dependents_to_activate.begin(), dependents_to_activate.end(), succ_pred) ==
728
- // dependents_to_activate.end();
729
- // });
730
- // }
731
- // // insert if not already in the list
732
- // std::copy_if(
733
- // predecessors_list.begin(), predecessors_list.end(),
734
- // std::back_inserter(dependents_to_activate),
735
- // [&dependents_to_activate](const std::string & predecessor)
736
- // {
737
- // return std::find(
738
- // dependents_to_activate.begin(), dependents_to_activate.end(), predecessor) ==
739
- // dependents_to_activate.end();
740
- // });
741
- // }
742
- // ros2_control::remove_item(dependents_to_activate, controller_name);
743
- // return dependents_to_activate;
744
- // }
745
-
746
- // private:
747
- // std::unordered_map<std::string, std::vector<std::string>> predecessors = {};
748
- // std::unordered_map<std::string, std::vector<std::string>> successors = {};
749
- // };
750
-
751
615
} // namespace controller_manager
752
616
#endif // CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_
0 commit comments