WIP Flexible actuator interfaces

extended_robot_hw_tests/CMakeLists.txt

@@ -0,0 +1,67 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(extended_robot_hw_tests)
+
+find_package(catkin REQUIRED
+  COMPONENTS
+    cmake_modules
+    pluginlib
+    transmission_interface)
+
+find_package(TinyXML REQUIRED)
+
+catkin_package(
+  LIBRARIES
+    ${PROJECT_NAME}_loader_plugins
+  INCLUDE_DIRS
+    include
+  CATKIN_DEPENDS
+    pluginlib
+  DEPENDS
+    TinyXML
+)
+
+
+###########
+## Build ##
+###########
+
+include_directories(include)
+include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${TinyXML_INCLUDE_DIRS})
+
+add_library(${PROJECT_NAME}_loader_plugins
+  src/extended_simple_transmission_loader.cpp
+  src/foo_joint_interface_provider.cpp)
+#  src/bar_joint_interface_provider.cpp)
+
+target_link_libraries(${PROJECT_NAME}_loader_plugins ${catkin_LIBRARIES})
+
+
+
+install(FILES extended_plugins.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+#############
+## Testing ##
+#############
+
+if(CATKIN_ENABLE_TESTING)
+
+  find_package(catkin REQUIRED
+    COMPONENTS
+      hardware_interface
+      resource_retriever
+      transmission_interface
+      urdf
+  )
+
+  find_package(rostest REQUIRED)
+
+  include_directories(SYSTEM ${catkin_INCLUDE_DIRS})
+
+  catkin_add_gtest(extended_robot_hw_test  test/extended_robot_hw_test.cpp)
+
+  target_link_libraries(extended_robot_hw_test
+      ${catkin_LIBRARIES})
+
+endif()
+

extended_robot_hw_tests/extended_plugins.xml

@@ -0,0 +1,11 @@
+<library path="lib/libextended_robot_hw_tests_loader_plugins">
+
+  <class name="transmission_interface/ExtendedSimpleTransmission"
+         type="transmission_interface::ExtendedSimpleTransmissionLoader"
+         base_class_type="transmission_interface::TransmissionLoader">
+    <description>
+      Load from a URDF description the configuration of a extended simple transmission.
+    </description>
+  </class>
+
+</library>

extended_robot_hw_tests/include/extended_robot_hw_tests/extended_joint_interface.h

@@ -0,0 +1,49 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2019, PAL Robotics S.L.
+//
+// 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 Jordán Palacios
+
+#ifndef HARDWARE_INTERFACE_EXTENDED_JOINT_INTERFACE_H
+#define HARDWARE_INTERFACE_EXTENDED_JOINT_INTERFACE_H
+
+#include <hardware_interface/joint_command_interface.h>
+
+namespace hardware_interface
+{
+
+// joint foo bar command interfaces
+class FooJointInterface : public JointCommandInterface
+{
+};
+
+class BarJointInterface : public JointCommandInterface
+{
+};
+
+}  // namespace hardware_interface
+
+#endif

extended_robot_hw_tests/include/extended_robot_hw_tests/extended_simple_transmission.h

@@ -0,0 +1,111 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2019, PAL Robotics S.L.
+//
+// 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 PAL Robotics S.L. 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 Jordán Palacios
+
+#ifndef TRANSMISSION_INTERFACE_EXTENDED_SIMPLE_TRANSMISSION_H
+#define TRANSMISSION_INTERFACE_EXTENDED_SIMPLE_TRANSMISSION_H
+
+#include <transmission_interface/simple_transmission.h>
+#include <transmission_interface/transmission_interface_exception.h>
+#include <transmission_interface/transmission.h>
+
+#include <vector>
+
+namespace transmission_interface
+{
+struct FooActuatorData
+{
+  std::vector<double*> foo;
+};
+
+struct BarActuatorData
+{
+  std::vector<double*> bar;
+};
+
+struct FooJointData
+{
+  std::vector<double*> foo;
+};
+
+struct BarJointData
+{
+  std::vector<double*> bar;
+};
+
+class ExtendedSimpleTransmission : public SimpleTransmission
+{
+public:
+  ExtendedSimpleTransmission(const double reduction, const double joint_offset = 0.0)
+    : SimpleTransmission(reduction, joint_offset), reduction_(reduction)
+  {
+  }
+
+  virtual ~ExtendedSimpleTransmission()
+  {
+  }
+
+  // bring base functions into scope
+  using SimpleTransmission::actuatorToJoint;
+  using SimpleTransmission::jointToActuator;
+
+  void actuatorToJoint(const FooActuatorData& act_data, FooJointData& jnt_data)
+  {
+    assert(numActuators() == act_data.foo.size() && numJoints() == jnt_data.foo.size());
+    assert(act_data.foo[0] && jnt_data.foo[0]);
+    *jnt_data.foo[0] = *act_data.foo[0] * reduction_;
+  }
+
+  void actuatorToJoint(const BarActuatorData& act_data, BarJointData& jnt_data)
+  {
+    assert(numActuators() == act_data.bar.size() && numJoints() == jnt_data.bar.size());
+    assert(act_data.bar[0] && jnt_data.bar[0]);
+    *jnt_data.bar[0] = *act_data.bar[0] * reduction_;
+  }
+
+  void jointToActuator(const FooJointData& jnt_data, FooActuatorData& act_data)
+  {
+    assert(numActuators() == act_data.foo.size() && numJoints() == jnt_data.foo.size());
+    assert(act_data.foo[0] && jnt_data.foo[0]);
+    *act_data.foo[0] = *jnt_data.foo[0] * reduction_;
+  }
+
+  void jointToActuator(const BarJointData& jnt_data, BarActuatorData& act_data)
+  {
+    assert(numActuators() == act_data.bar.size() && numJoints() == jnt_data.bar.size());
+    assert(act_data.bar[0] && jnt_data.bar[0]);
+    *act_data.bar[0] = *jnt_data.bar[0] * reduction_;
+  }
+
+private:
+  double reduction_;
+};
+
+}  // namespace transmission_interface
+
+#endif  // TRANSMISSION_INTERFACE_EXTENDED_SIMPLE_TRANSMISSION_H

extended_robot_hw_tests/include/extended_robot_hw_tests/extended_simple_transmission_loader.h

@@ -0,0 +1,45 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2019, PAL Robotics S.L.
+//
+// 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 PAL Robotics S.L. 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 Jordán Palacios
+
+#ifndef TRANSMISSION_INTERFACE_EXTENDED_SIMPLE_TRANSMISSION_LOADER_H
+#define TRANSMISSION_INTERFACE_EXTENDED_SIMPLE_TRANSMISSION_LOADER_H
+
+#include <transmission_interface/transmission_loader.h>
+
+namespace transmission_interface
+{
+class ExtendedSimpleTransmissionLoader : public TransmissionLoader
+{
+public:
+  TransmissionSharedPtr load(const TransmissionInfo& transmission_info);
+};
+
+}  // namespace transmission_interface
+
+#endif

extended_robot_hw_tests/include/extended_robot_hw_tests/foo_joint_interface_provider.h

@@ -0,0 +1,61 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2019, PAL Robotics S.L.
+//
+// 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 PAL Robotics S.L. 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 Jordán Palacios
+
+#ifndef TRANSMISSION_INTERFACE_FOO_JOINT_INTERFACE_PROVIDER_H
+#define TRANSMISSION_INTERFACE_FOO_JOINT_INTERFACE_PROVIDER_H
+
+// ros_control
+#include <transmission_interface/transmission_info.h>
+#include <transmission_interface/transmission_interface_loader.h>
+#include <transmission_interface/joint_state_interface_provider.h>
+
+namespace transmission_interface
+{
+class FooJointInterfaceProvider : public JointStateInterfaceProvider
+{
+public:
+  bool updateJointInterfaces(const TransmissionInfo& transmission_info,
+                             hardware_interface::RobotHW* robot_hw,
+                             JointInterfaces& joint_interfaces,
+                             RawJointDataMap& raw_joint_data_map);
+
+protected:
+  bool getJointCommandData(const TransmissionInfo& transmission_info,
+                           const RawJointDataMap& raw_joint_data_map, JointData& jnt_cmd_data);
+
+  bool getActuatorCommandData(const TransmissionInfo& transmission_info,
+                              hardware_interface::RobotHW* robot_hw, ActuatorData& act_cmd_data);
+
+  bool registerTransmission(TransmissionLoaderData& loader_data,
+                            TransmissionHandleData& handle_data);
+};
+
+}  // namespace transmission_interface
+
+#endif

extended_robot_hw_tests/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>extended_robot_hw_tests</name>
+  <version>0.15.1</version>
+  <description>Robot HW Extension tests</description>
+  <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
+
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>pluginlib</build_depend>
+  <depend>transmission_interface</depend>
+
+  <exec_depend>pluginlib</exec_depend>
+
+  <test_depend>hardware_interface</test_depend>
+  <test_depend>resource_retriever</test_depend>
+  <test_depend>rostest</test_depend>
+  <test_depend>rosunit</test_depend>
+  <test_depend>urdf</test_depend>
+
+  <export>
+	  <transmission_interface plugin="${prefix}/extended_plugins.xml" />
+  </export>
+
+</package>