RobotHWGroup with aliased interfaces

controller_manager_tests/CMakeLists.txt

@@ -45,18 +45,27 @@ else()
     include/controller_manager_tests/my_robot_hw.h
     src/effort_test_controller.cpp
     include/controller_manager_tests/effort_test_controller.h
+    src/vel_effort_test_controller.cpp
+    include/controller_manager_tests/vel_effort_test_controller.h
     src/my_dummy_controller.cpp
     include/controller_manager_tests/my_dummy_controller.h
     )
 
   add_executable(dummy_app src/dummy_app.cpp)
   target_link_libraries(dummy_app ${PROJECT_NAME} ${catkin_LIBRARIES})
+  add_executable(multi_dummy_app src/multi_dummy_app.cpp)
+  target_link_libraries(multi_dummy_app ${PROJECT_NAME} ${catkin_LIBRARIES})
 
   if(CATKIN_ENABLE_TESTING)
     add_executable(cm_test test/cm_test.cpp)
     add_dependencies(tests cm_test)
     target_link_libraries(cm_test ${GTEST_LIBRARIES} ${catkin_LIBRARIES})
     add_rostest(test/cm_test.test)
+
+    add_executable(multi_cm_test test/multi_cm_test.cpp)
+    add_dependencies(tests multi_cm_test)
+    target_link_libraries(multi_cm_test ${GTEST_LIBRARIES} ${catkin_LIBRARIES})
+    add_rostest(test/multi_cm_test.test)
   endif()
 
   # Install

controller_manager_tests/include/controller_manager_tests/multi_robot_hw.h

@@ -0,0 +1,70 @@
+// Author: Kelsey Hawkins
+// Based on code by:
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2012, hiDOF INC.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//   * Redistributions of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//   * Redistributions in binary form must reproduce the above copyright
+//     notice, this list of conditions and the following disclaimer in the
+//     documentation and/or other materials provided with the distribution.
+//   * Neither the name of hiDOF, Inc. nor the names of its
+//     contributors may be used to endorse or promote products derived from
+//     this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+/*
+ * Author: Wim Meeussen, Kelsey Hawkins
+ */
+
+
+#ifndef CONTROLLER_MANAGER_TESTS_MULTI_ROBOT_HW_H
+#define CONTROLLER_MANAGER_TESTS_MULTI_ROBOT_HW_H
+
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/robot_hw.h>
+
+namespace controller_manager_tests
+{
+
+// Used to test multiple robot hardware mixed together
+class TypeARobotHW : public hardware_interface::RobotHW
+{
+public:
+  TypeARobotHW(int id);
+
+  void read();
+  void write();
+
+protected:
+
+private:
+  hardware_interface::JointStateInterface    js_interface_;
+  hardware_interface::EffortJointInterface   ej_interface_;
+  hardware_interface::VelocityJointInterface vj_interface_;
+
+  std::vector<double> joint_effort_command_;
+  std::vector<double> joint_velocity_command_;
+  std::vector<double> joint_position_;
+  std::vector<double> joint_velocity_;
+  std::vector<double> joint_effort_;
+  std::vector<std::string> joint_name_;
+};
+}
+
+
+#endif

controller_manager_tests/include/controller_manager_tests/my_robot_hw.h

@@ -42,7 +42,7 @@ namespace controller_manager_tests
 class MyRobotHW : public hardware_interface::RobotHW
 {
 public:
-  MyRobotHW();
+  MyRobotHW(std::string joint_prefix = "hiDOF_");
 
   void read();
   void write();
@@ -61,6 +61,14 @@ class MyRobotHW : public hardware_interface::RobotHW
   std::vector<double> joint_effort_;
   std::vector<std::string> joint_name_;
 };
+
+class DerivedMyRobotHW : public MyRobotHW 
+{
+public:
+  DerivedMyRobotHW(std::string joint_prefix = "hiDOF_")
+    : MyRobotHW(joint_prefix + "derived_") {}
+};
+
 }
 
 

controller_manager_tests/include/controller_manager_tests/vel_effort_test_controller.h

@@ -0,0 +1,65 @@
+// Author: Kelsey Hawkins
+// Based on code by:
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2012, hiDOF INC.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//   * Redistributions of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//   * Redistributions in binary form must reproduce the above copyright
+//     notice, this list of conditions and the following disclaimer in the
+//     documentation and/or other materials provided with the distribution.
+//   * Neither the name of hiDOF Inc nor the names of its
+//     contributors may be used to endorse or promote products derived from
+//     this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+#ifndef CONTROLLER_MANAGER_TESTS_VEL_EFFORT_TEST_CONTROLLER_H
+#define CONTROLLER_MANAGER_TESTS_VEL_EFFORT_TEST_CONTROLLER_H
+
+
+#include <controller_interface/multi_controller.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <pluginlib/class_list_macros.h>
+
+
+namespace controller_manager_tests
+{
+
+
+class VelocityEffortTestController: 
+  public controller_interface::Controller2<hardware_interface::VelocityJointInterface,
+                                           hardware_interface::EffortJointInterface>
+{
+public:
+  VelocityEffortTestController(){}
+
+  bool init(hardware_interface::VelocityJointInterface* hw_vel, 
+            hardware_interface::EffortJointInterface* hw_eff, 
+            ros::NodeHandle &n);
+  void starting(const ros::Time& time);
+  void update(const ros::Time& time, const ros::Duration& period);
+  void stopping(const ros::Time& time);
+
+private:
+  std::vector<hardware_interface::JointHandle> joint_velocity_commands_;
+  std::vector<hardware_interface::JointHandle> joint_effort_commands_;
+
+};
+
+}
+
+#endif

controller_manager_tests/src/multi_dummy_app.cpp

@@ -0,0 +1,70 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2012, hiDOF INC.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//   * Redistributions of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//   * Redistributions in binary form must reproduce the above copyright
+//     notice, this list of conditions and the following disclaimer in the
+//     documentation and/or other materials provided with the distribution.
+//   * Neither the name of hiDOF Inc nor the names of its
+//     contributors may be used to endorse or promote products derived from
+//     this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+#include <ros/ros.h>
+#include <controller_manager/controller_manager.h>
+#include <controller_manager_tests/my_robot_hw.h>
+
+using namespace controller_manager_tests;
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "MultiDummyApp");
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+
+  MyRobotHW hw_vel_base("vel_");
+  MyRobotHW hw_eff_base("eff_");
+  DerivedMyRobotHW hw_vel_derived("vel_");
+  DerivedMyRobotHW hw_eff_derived("eff_");
+  hardware_interface::RobotHW hw_combined;
+  hw_combined.registerInterfaceManager(&hw_vel_base);
+  hw_combined.registerInterfaceManager(&hw_eff_base);
+  hw_combined.registerInterfaceManager(&hw_vel_derived);
+  hw_combined.registerInterfaceManager(&hw_eff_derived);
+
+  ros::NodeHandle nh;
+  controller_manager::ControllerManager cm(&hw_combined, nh);
+
+  ros::Duration period(1.0);
+  while (ros::ok())
+  {
+    ROS_INFO("loop");
+    hw_vel_base.read();
+    hw_eff_base.read();
+    hw_vel_derived.read();
+    hw_eff_derived.read();
+    cm.update(ros::Time::now(), period);
+    hw_vel_base.write();
+    hw_eff_base.write();
+    hw_vel_derived.write();
+    hw_eff_derived.write();
+    period.sleep();
+  }
+}
+

controller_manager_tests/src/multi_robot_hw.cpp

@@ -0,0 +1,91 @@
+// Author: Kelsey Hawkins
+// Based on code by:
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2012, hiDOF INC.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//   * Redistributions of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//   * Redistributions in binary form must reproduce the above copyright
+//     notice, this list of conditions and the following disclaimer in the
+//     documentation and/or other materials provided with the distribution.
+//   * Neither the name of hiDOF Inc nor the names of its
+//     contributors may be used to endorse or promote products derived from
+//     this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+/*
+ * Author: Wim Meeussen, Kelsey Hawkins
+ */
+
+
+#include <controller_manager_tests/my_robot_hw.h>
+
+namespace controller_manager_tests
+{
+
+TypeARobotHW::TypeARobotHW(int id)
+{
+  using namespace hardware_interface;
+
+  // Initialize raw data
+  joint_position_.resize(2);
+  joint_velocity_.resize(2);
+  joint_effort_.resize(2);
+  joint_effort_command_.resize(2);
+  joint_velocity_command_.resize(2);
+  joint_name_.resize(2);
+
+  joint_name_[0] = "hiDOF_joint1";
+  joint_position_[0] = 1.0;
+  joint_velocity_[0] = 0.0;
+  joint_effort_[0] = 0.1;
+  joint_effort_command_[0] = 0.0;
+  joint_velocity_command_[0] = 0.0;
+
+  joint_name_[1] = "hiDOF_joint2";
+  joint_position_[1] = 1.0;
+  joint_velocity_[1] = 0.0;
+  joint_effort_[1] = 0.1;
+  joint_effort_command_[1] = 0.0;
+  joint_velocity_command_[1] = 0.0;
+
+  // Populate hardware interfaces
+  js_interface_.registerHandle(JointStateHandle(joint_name_[0], &joint_position_[0], &joint_velocity_[0], &joint_effort_[0]));
+  js_interface_.registerHandle(JointStateHandle(joint_name_[1], &joint_position_[1], &joint_velocity_[1], &joint_effort_[1]));
+
+  ej_interface_.registerHandle(JointHandle(js_interface_.getHandle(joint_name_[0]), &joint_effort_command_[0]));
+  ej_interface_.registerHandle(JointHandle(js_interface_.getHandle(joint_name_[1]), &joint_effort_command_[1]));
+
+  vj_interface_.registerHandle(JointHandle(js_interface_.getHandle(joint_name_[0]), &joint_velocity_command_[0]));
+  vj_interface_.registerHandle(JointHandle(js_interface_.getHandle(joint_name_[1]), &joint_velocity_command_[1]));
+
+  registerInterface(&js_interface_);
+  registerInterface(&ej_interface_);
+  registerInterface(&vj_interface_);
+}
+
+
+void TypeARobotHW::read()
+{
+
+}
+
+void TypeARobotHW::write()
+{
+}
+
+}

controller_manager_tests/src/my_robot_hw.cpp

@@ -35,7 +35,7 @@
 namespace controller_manager_tests
 {
 
-MyRobotHW::MyRobotHW()
+MyRobotHW::MyRobotHW(std::string joint_prefix)
 {
   using namespace hardware_interface;
 
@@ -47,14 +47,14 @@ MyRobotHW::MyRobotHW()
   joint_velocity_command_.resize(2);
   joint_name_.resize(2);
 
-  joint_name_[0] = "hiDOF_joint1";
+  joint_name_[0] = joint_prefix + "joint1";
   joint_position_[0] = 1.0;
   joint_velocity_[0] = 0.0;
   joint_effort_[0] = 0.1;
   joint_effort_command_[0] = 0.0;
   joint_velocity_command_[0] = 0.0;
 
-  joint_name_[1] = "hiDOF_joint2";
+  joint_name_[1] = joint_prefix + "joint2";
   joint_position_[1] = 1.0;
   joint_velocity_[1] = 0.0;
   joint_effort_[1] = 0.1;