diff --git a/moveit_setup_assistant/moveit_setup_controllers/src/controllers_config.cpp b/moveit_setup_assistant/moveit_setup_controllers/src/controllers_config.cpp index 25962cf130..10689c3341 100644 --- a/moveit_setup_assistant/moveit_setup_controllers/src/controllers_config.cpp +++ b/moveit_setup_assistant/moveit_setup_controllers/src/controllers_config.cpp @@ -1,4 +1,5 @@ /********************************************************************* + * Software License Agreement (BSD License) * Software License Agreement (BSD License) * * Copyright (c) 2022, Metro Robots @@ -32,12 +33,15 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ + /* Author: David V. Lu!! */ + #include #include #include + namespace moveit_setup { namespace controllers @@ -52,26 +56,32 @@ bool ControllersConfig::addController(const std::string& name, const std::string return addController(controller); } + bool ControllersConfig::addController(const ControllerInfo& new_controller) { // Used for holding our search results ControllerInfo* searched_ros_controller = nullptr; + // Find if there is an existing controller with the same name searched_ros_controller = findControllerByName(new_controller.name_); - if (searched_ros_controller && searched_ros_controller->type_ == new_controller.type_) + + if (searched_ros_controller) return false; + controllers_.push_back(new_controller); return true; } + ControllerInfo* ControllersConfig::findControllerByName(const std::string& controller_name) { // Find the Controller we are editing based on the Controller name string ControllerInfo* searched_ros_controller = nullptr; // used for holding our search results + for (ControllerInfo& ros_control_config : controllers_) { if (ros_control_config.name_ == controller_name) // string match @@ -81,9 +91,11 @@ ControllerInfo* ControllersConfig::findControllerByName(const std::string& contr } } + return searched_ros_controller; } + bool ControllersConfig::deleteController(const std::string& controller_name) { for (std::vector::iterator controller_it = controllers_.begin(); controller_it != controllers_.end();