From acababcd2e2b6159d0e10c4f723f2bcbb97fac4d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Mathias=20L=C3=BCdtke?= <mathias.luedtke@ipa.fraunhofer.de>
Date: Fri, 17 Feb 2017 18:35:52 +0100
Subject: [PATCH] add multi interface wheel controller

---
 cob_omni_drive_controller/CMakeLists.txt      |  2 +-
 .../controller_plugins.xml                    |  4 ++
 .../src/control_multi_plugin.cpp              | 52 +++++++++++++++++++
 3 files changed, 57 insertions(+), 1 deletion(-)
 create mode 100644 cob_omni_drive_controller/src/control_multi_plugin.cpp

diff --git a/cob_omni_drive_controller/CMakeLists.txt b/cob_omni_drive_controller/CMakeLists.txt
index 3c0e4520..93c85c22 100644
--- a/cob_omni_drive_controller/CMakeLists.txt
+++ b/cob_omni_drive_controller/CMakeLists.txt
@@ -31,7 +31,7 @@ add_library(cob_omni_drive_geom src/UndercarriageCtrlGeom.cpp src/param_parser.c
 add_dependencies(cob_omni_drive_geom ${catkin_EXPORTED_TARGETS})
 target_link_libraries(cob_omni_drive_geom ${catkin_LIBRARIES})
 
-add_library(cob_omni_drive_controller src/odom_plugin.cpp src/control_plugin.cpp)
+add_library(cob_omni_drive_controller src/odom_plugin.cpp src/control_plugin.cpp src/control_multi_plugin.cpp)
 add_dependencies(cob_omni_drive_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(cob_omni_drive_controller cob_omni_drive_geom ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
diff --git a/cob_omni_drive_controller/controller_plugins.xml b/cob_omni_drive_controller/controller_plugins.xml
index f5b33f13..51468180 100644
--- a/cob_omni_drive_controller/controller_plugins.xml
+++ b/cob_omni_drive_controller/controller_plugins.xml
@@ -8,5 +8,9 @@
             <description>
             </description>
         </class>
+        <class name="cob_omni_drive_controller/WheelMultiController" type="cob_omni_drive_controller::WheelMultiController" base_class_type="controller_interface::ControllerBase">
+            <description>
+            </description>
+        </class>
     </library>
 </class_libraries>
diff --git a/cob_omni_drive_controller/src/control_multi_plugin.cpp b/cob_omni_drive_controller/src/control_multi_plugin.cpp
new file mode 100644
index 00000000..6edab033
--- /dev/null
+++ b/cob_omni_drive_controller/src/control_multi_plugin.cpp
@@ -0,0 +1,52 @@
+#include "GeomController.h"
+#include "WheelControllerBase.h"
+#include <controller_interface/multi_interface_controller.h>
+#include <hardware_interface/joint_command_interface.h>
+
+namespace cob_omni_drive_controller
+{
+
+class GeomMultiController : public GeomControllerBase< hardware_interface::JointHandle, UndercarriageDirectCtrl>,
+    public controller_interface::MultiInterfaceController<hardware_interface::VelocityJointInterface, hardware_interface::PositionJointInterface> {
+};
+
+class WheelMultiController : public WheelControllerBase< GeomMultiController >
+{
+public:
+    virtual bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle &root_nh, ros::NodeHandle& controller_nh){
+        std::vector<UndercarriageDirectCtrl::WheelParams> wheel_params;
+        if(!parseWheelParams(wheel_params, controller_nh)) return false;
+        if(!GeomControllerBase::setup(wheel_params)) return false;
+
+        hardware_interface::VelocityJointInterface* v = robot_hw->get<hardware_interface::VelocityJointInterface>();
+        hardware_interface::PositionJointInterface* p = robot_hw->get<hardware_interface::PositionJointInterface>();
+
+        try{
+            for (unsigned i=0; i<wheel_params.size(); i++){
+                this->steer_joints_.push_back(p->getHandle(wheel_params[i].geom.steer_name));
+                this->drive_joints_.push_back(v->getHandle(wheel_params[i].geom.drive_name));
+            }
+        }
+        catch(const std::exception &e){
+            ROS_ERROR_STREAM("Error while attaching handles: " << e.what());
+            return false;
+        }
+        return this->setup(root_nh,controller_nh);
+    }
+    virtual void update(const ros::Time& time, const ros::Duration& period){
+
+        updateState();
+
+        updateCtrl(time, period);
+
+        for (unsigned i=0; i<wheel_commands_.size(); i++){
+            steer_joints_[i].setCommand(wheel_commands_[i].dAngGearSteerRad);
+            drive_joints_[i].setCommand(wheel_commands_[i].dVelGearDriveRadS);
+        }
+
+    }
+};
+
+}
+
+PLUGINLIB_EXPORT_CLASS( cob_omni_drive_controller::WheelMultiController, controller_interface::ControllerBase)