From ccd33a3c9cc102adbdd8638e079c59efe919b565 Mon Sep 17 00:00:00 2001
From: Michael Ferguson <mfergs7@gmail.com>
Date: Wed, 17 Jun 2020 18:30:53 -0400
Subject: [PATCH] implement getJointNames/getControllerNames

this was discussed in #39
---
 .../controller_manager.h                      | 10 ++++++++++
 .../src/controller_manager.cpp                | 20 +++++++++++++++++++
 2 files changed, 30 insertions(+)

diff --git a/robot_controllers_interface/include/robot_controllers_interface/controller_manager.h b/robot_controllers_interface/include/robot_controllers_interface/controller_manager.h
index 45a170f..7261e9f 100644
--- a/robot_controllers_interface/include/robot_controllers_interface/controller_manager.h
+++ b/robot_controllers_interface/include/robot_controllers_interface/controller_manager.h
@@ -110,6 +110,16 @@ class ControllerManager : public std::enable_shared_from_this<ControllerManager>
    */
   GyroHandlePtr getGyroHandle(const std::string& name);
 
+  /**
+   * @brief Get the names of all joints.
+   */
+  std::vector<std::string> getJointNames();
+
+  /**
+   * @brief Get the names of all controllers.
+   */
+  std::vector<std::string> getControllerNames();
+
 private:
   /** @brief Service callback */
   void callback(
diff --git a/robot_controllers_interface/src/controller_manager.cpp b/robot_controllers_interface/src/controller_manager.cpp
index ae3932c..e469923 100644
--- a/robot_controllers_interface/src/controller_manager.cpp
+++ b/robot_controllers_interface/src/controller_manager.cpp
@@ -289,6 +289,26 @@ GyroHandlePtr ControllerManager::getGyroHandle(const std::string& name)
   return GyroHandlePtr();
 }
 
+std::vector<std::string> ControllerManager::getJointNames()
+{
+  std::vector<std::string> names;
+  for (auto j = joints_.begin(); j != joints_.end(); ++j)
+  {
+    names.push_back((*j)->getName());
+  }
+  return names;
+}
+
+std::vector<std::string> ControllerManager::getControllerNames()
+{
+  std::vector<std::string> names;
+  for (auto c = controllers_.begin(); c != controllers_.end(); ++c)
+  {
+    names.push_back((*c)->getController()->getName());
+  }
+  return names;
+}
+
 void ControllerManager::callback(
     const std::shared_ptr<robot_controllers_msgs::srv::QueryControllerStates::Request> request,
     std::shared_ptr<robot_controllers_msgs::srv::QueryControllerStates::Response> response)