35 #ifndef TRANSMISSION_INTERFACE_TRANSMISSION_INTERFACE_H
36 #define TRANSMISSION_INTERFACE_TRANSMISSION_INTERFACE_H
42 #include <hardware_interface/internal/resource_manager.h>
43 #include <transmission_interface/transmission.h>
45 #include <transmission_interface/transmission_interface_exception.h>
54 class MiaTransmissionHandle
82 const ActuatorData& actuator_data,
83 const JointData& joint_data,
84 const ActuatorData& actuator_state)
94 throw TransmissionInterfaceException(
"Unspecified transmission.");
98 if (actuator_data.position.empty() && actuator_data.velocity.empty() && actuator_data.effort.empty() &&
99 joint_data.position.empty() && joint_data.velocity.empty() && joint_data.effort.empty() &&
100 actuator_state.position.empty() && actuator_state.velocity.empty() && actuator_state.effort.empty())
102 throw TransmissionInterfaceException(
"All data vectors are empty. Transmission instance can't do anything!.");
106 if (!actuator_data.position.empty() && actuator_data.position.size() !=
transmission_->numActuators() )
108 throw TransmissionInterfaceException(
"Actuator position data size does not match transmission.");
110 if (!actuator_data.velocity.empty() && actuator_data.velocity.size() !=
transmission_->numActuators())
112 throw TransmissionInterfaceException(
"Actuator velocity data size does not match transmission.");
114 if (!actuator_data.effort.empty() && actuator_data.effort.size() !=
transmission_->numActuators())
116 throw TransmissionInterfaceException(
"Actuator effort data size does not match transmission.");
119 if (!joint_data.position.empty() && joint_data.position.size() !=
transmission_->numJoints())
121 throw TransmissionInterfaceException(
"Joint position data size does not match transmission.");
123 if (!joint_data.velocity.empty() && joint_data.velocity.size() !=
transmission_->numJoints())
125 throw TransmissionInterfaceException(
"Joint velocity data size does not match transmission.");
127 if (!joint_data.effort.empty() && joint_data.effort.size() !=
transmission_->numJoints())
129 throw TransmissionInterfaceException(
"Joint effort data size does not match transmission.");
133 if (!actuator_state.position.empty() && actuator_state.position.size() !=
transmission_->numActuators() )
135 throw TransmissionInterfaceException(
"Actuator position state size does not match transmission.");
137 if (!actuator_state.velocity.empty() && actuator_state.velocity.size() !=
transmission_->numActuators())
139 throw TransmissionInterfaceException(
"Actuator velocity state size does not match transmission.");
141 if (!actuator_state.effort.empty() && actuator_state.effort.size() !=
transmission_->numActuators())
143 throw TransmissionInterfaceException(
"Actuator effort state size does not match transmission.");
150 throw TransmissionInterfaceException(
"Actuator position data contains null pointers.");
154 throw TransmissionInterfaceException(
"Actuator velocity data contains null pointers.");
158 throw TransmissionInterfaceException(
"Actuator effort data contains null pointers.");
163 throw TransmissionInterfaceException(
"Joint position data contains null pointers.");
167 throw TransmissionInterfaceException(
"Joint velocity data contains null pointers.");
171 throw TransmissionInterfaceException(
"Joint effort data contains null pointers.");
177 throw TransmissionInterfaceException(
"Actuator position state contains null pointers.");
181 throw TransmissionInterfaceException(
"Actuator velocity state contains null pointers.");
185 throw TransmissionInterfaceException(
"Actuator effort state contains null pointers.");
192 for (std::vector<double*>::const_iterator it = data.begin(); it != data.end(); ++it)
194 if (!(*it)) {
return false;}
204 class MiaActuatorToJointStateHandle :
public MiaTransmissionHandle
217 const ActuatorData& actuator_data,
218 const JointData& joint_data)
250 const ActuatorData& actuator_data,
251 const JointData& joint_data)
265 class MiaActuatorToJointVelocityHandle :
public MiaTransmissionHandle
278 const ActuatorData& actuator_data,
279 const JointData& joint_data)
293 class MiaJointToActuatorStateHandle :
public MiaTransmissionHandle
306 const ActuatorData& actuator_data,
307 const JointData& joint_data,
308 const ActuatorData& actuator_state)
342 const ActuatorData& actuator_data,
343 const JointData& joint_data,
344 const ActuatorData& actuator_state)
359 class MiaJointToActuatorVelocityHandle :
public MiaTransmissionHandle
372 const ActuatorData& actuator_data,
373 const JointData& joint_data,
374 const ActuatorData& actuator_state)
383 template <
class HandleType>
384 class MiaTransmissionInterface :
public hardware_interface::ResourceManager<HandleType>
388 HandleType
getHandle(
const std::string& name)
393 return this->hardware_interface::ResourceManager<HandleType>::getHandle(name);
395 catch(
const std::logic_error& e)
397 throw TransmissionInterfaceException(e.what());
403 typedef typename hardware_interface::ResourceManager<HandleType>::ResourceMap::iterator ItratorType;
404 for (ItratorType it = this->resource_map_.begin(); it != this->resource_map_.end(); ++it)
406 it->second.propagate();
414 class MiaActuatorToJointStateInterface :
public MiaTransmissionInterface<MiaActuatorToJointStateHandle> {};
416 class MiaActuatorToJointPositionInterface :
public MiaTransmissionInterface<MiaActuatorToJointPositionHandle> {};
418 class MiaActuatorToJointVelocityInterface :
public MiaTransmissionInterface<MiaActuatorToJointVelocityHandle> {};
430 #endif //TRANSMISSION_INTERFACE_TRANSMISSION_INTERFACE_H