Allow multiple hw interfaces, Fix ros-controls#112, and test.

- Allow to specify multiple hardware interfaces for joints and actuators. - Fix invalid xml_element tag. Contents are now stored as a string. - Unit test parser.
pal-robotics-forks · Dec 12, 2019 · ef04d22 · ef04d22
1 parent bcfc590
commit ef04d22
Show file tree

Hide file tree

Showing 4 changed files with 46 additions and 0 deletions.
diff --git a/transmission_interface/CMakeLists.txt b/transmission_interface/CMakeLists.txt
@@ -66,6 +66,23 @@ add_library(${PROJECT_NAME}_loader_plugins
   src/bidirectional_effort_joint_interface_provider.cpp   include/transmission_interface/bidirectional_effort_joint_interface_provider.h)
 target_link_libraries(${PROJECT_NAME}_loader_plugins ${PROJECT_NAME}_loader)
 
+<<<<<<< HEAD
+=======
+  # Transmission parser Library
+  add_library(${PROJECT_NAME}_parser
+    src/transmission_parser.cpp include/transmission_interface/transmission_parser.h
+  )
+  target_link_libraries(${PROJECT_NAME}_parser ${catkin_LIBRARIES} ${TinyXML_LIBRARIES})
+>>>>>>> First draft of transmission loading.
+
+  # Transmission loader library
+  add_library(transmission_interface_loader
+    src/transmission_loader.cpp           include/transmission_interface/transmission_loader.h
+    src/simple_transmission_loader.cpp    include/transmission_interface/simple_transmission_loader.h
+    src/transmission_interface_loader.cpp include/transmission_interface/transmission_interface_loader.h
+  )
+  target_link_libraries(transmission_interface_loader ${catkin_LIBRARIES} ${TinyXML_LIBRARIES})
+
 
 #############
 ## Install ##

diff --git a/transmission_interface/include/transmission_interface/transmission_interface_loader.h b/transmission_interface/include/transmission_interface/transmission_interface_loader.h
@@ -245,6 +245,8 @@ class RequisiteProvider // TODO: There must be a more descriptive name for this
     }
   }
 
+  static unsigned int addJoint(const std::string& name, RawJointData& raw_joint_data);
+
   template <class HardwareInterface, class Handle>
   bool getActuatorHandles(const std::vector<ActuatorInfo>& actuators_info,
                           hardware_interface::RobotHW*     robot_hw,

diff --git a/transmission_interface/src/position_joint_interface_provider.cpp b/transmission_interface/src/position_joint_interface_provider.cpp
@@ -60,7 +60,11 @@ bool PositionJointInterfaceProvider::updateJointInterfaces(const TransmissionInf
     const std::string& name = joint_info.name_;
 
     // Do nothing if joint already exists on the hardware interface
+<<<<<<< HEAD
     if (hasResource(name, interface)) {continue;}
+=======
+    if (hasResource(name, interface)) {return true;}
+>>>>>>> Fix bug when adding multiple transmissions.
 
     // Update hardware interface
     using hardware_interface::JointHandle;

diff --git a/transmission_interface/src/transmission_interface_loader.cpp b/transmission_interface/src/transmission_interface_loader.cpp
@@ -193,7 +193,30 @@ bool TransmissionInterfaceLoader::load(const TransmissionInfo& transmission_info
                            "'. It contains no valid hardware interfaces.");
     return false;
   }
+  return ret;
+}
 
+bool JointStateInterfaceProvider::updateJointInterfaces(const TransmissionInfo& transmission_info,
+                                                        JointInterfaces&        joint_interfaces,
+                                                        RawJointData&           raw_joint_data)
+{
+  // Register joint on all its hardware interfaces
+  BOOST_FOREACH(const JointInfo& joint_info, transmission_info.joints_)
+  {
+    const std::string& name = joint_info.name_;
+
+    // Do nothing if joint already exists on the hardware interface
+    if (hasResource(name, joint_interfaces.joint_state_interface)) {return true;}
+
+    // Update hardware interface
+    using hardware_interface::JointStateHandle;
+    const unsigned int id = addJoint(name, raw_joint_data);
+    JointStateHandle handle(raw_joint_data.names[id],
+                            &raw_joint_data.position[id],
+                            &raw_joint_data.velocity[id],
+                            &raw_joint_data.effort[id]);
+    joint_interfaces.joint_state_interface.registerHandle(handle);
+  }
   return true;
 }