45 if (!robot_hw->
get<JointStateInterface>())
49 JointStateInterface&
interface = *(robot_hw->
get<JointStateInterface>());
52 for (
const auto& joint_info : transmission_info.
joints_)
54 const std::string& name = joint_info.name_;
64 JointStateHandle handle(name,
70 interface.registerHandle(handle);
74 JointStateHandle handle(name,
79 interface.registerHandle(handle);
83 JointStateHandle handle(name,
88 interface.registerHandle(handle);
92 JointStateHandle handle(name,
96 interface.registerHandle(handle);
107 const unsigned int dim = transmission_info.
joints_.size();
108 jnt_state_data.
position.resize(dim);
109 jnt_state_data.
velocity.resize(dim);
110 jnt_state_data.
effort.resize(dim);
112 bool hasAbsolutePosition =
true;
113 bool hasTorqueSensor =
true;
115 for (
unsigned int i = 0; i < dim; ++i)
117 const std::string& joint_name = transmission_info.
joints_[i].name_;
118 RawJointDataMap::const_iterator raw_joint_data_it = raw_joint_data_map.find(joint_name);
119 if (raw_joint_data_it == raw_joint_data_map.end()) {
return false;}
120 const RawJointData& raw_joint_data = raw_joint_data_it->second;
126 if(hasAbsolutePosition)
136 for (
unsigned int i = 0; i < dim; ++i)
138 const std::string& joint_name = transmission_info.
joints_[i].name_;
139 RawJointDataMap::const_iterator raw_joint_data_it = raw_joint_data_map.find(joint_name);
140 if (raw_joint_data_it == raw_joint_data_map.end()) {
return false;}
141 const RawJointData& raw_joint_data = raw_joint_data_it->second;
144 jnt_state_data.
position[i] =
const_cast<double*
>(&(raw_joint_data.
position));
145 jnt_state_data.
velocity[i] =
const_cast<double*
>(&(raw_joint_data.
velocity));
146 jnt_state_data.
effort[i] =
const_cast<double*
>(&(raw_joint_data.
effort));
147 if(hasAbsolutePosition)
168 std::vector<ActuatorStateHandle> handles;
169 if (!this->getActuatorHandles<ActuatorStateInterface, ActuatorStateHandle>(transmission_info.
actuators_,
171 handles)) {
return false;}
174 const unsigned int dim = transmission_info.
actuators_.size();
175 act_state_data.
position.resize(dim);
176 act_state_data.
velocity.resize(dim);
177 act_state_data.
effort.resize(dim);
179 bool hasAbsolutePositionInterface =
true;
180 bool hasTorqueSensorInterface =
true;
182 for (
unsigned int i = 0; i < dim; ++i)
184 hasAbsolutePositionInterface = hasAbsolutePositionInterface && handles[i].hasAbsolutePosition();
185 hasTorqueSensorInterface = hasTorqueSensorInterface && handles[i].hasTorqueSensor();
188 if(hasAbsolutePositionInterface)
193 if(hasTorqueSensorInterface)
198 for (
unsigned int i = 0; i < dim; ++i)
201 act_state_data.
position[i] =
const_cast<double*
>(handles[i].getPositionPtr());
202 act_state_data.
velocity[i] =
const_cast<double*
>(handles[i].getVelocityPtr());
203 act_state_data.
effort[i] =
const_cast<double*
>(handles[i].getEffortPtr());
204 if(hasAbsolutePositionInterface)
206 act_state_data.
absolute_position[i] =
const_cast<double*
>(handles[i].getAbsolutePositionPtr());
208 if(hasTorqueSensorInterface)
210 act_state_data.
torque_sensor[i] =
const_cast<double*
>(handles[i].getTorqueSensorPtr());
231 interface.registerHandle(handle);
void registerInterface(T *iface)
TransmissionSharedPtr transmission
std::vector< double * > absolute_position
std::vector< double * > velocity
std::vector< double * > absolute_position
Contains pointers to raw data representing the position, velocity and acceleration of a transmission'...
std::vector< double * > position
std::vector< double * > torque_sensor
Contains semantic info about a given transmission loaded from XML (URDF)
std::vector< double * > torque_sensor
Joint interfaces of a robot. Only used interfaces need to be populated.
bool registerTransmission(TransmissionLoaderData &loader_data, TransmissionHandleData &handle_data) override
Handle for propagating actuator state (position, velocity and effort) to joint state for a given tran...
ForwardTransmissionInterfaces transmission_interfaces
static bool hasResource(const std::string &name, const Interface &iface)
bool getActuatorStateData(const TransmissionInfo &transmission_info, hardware_interface::RobotHW *robot_hw, ActuatorData &act_state_data) override
hardware_interface::JointStateInterface joint_state_interface
bool getJointStateData(const TransmissionInfo &transmission_info, const RawJointDataMap &raw_joint_data_map, JointData &jnt_state_data) override
Contains pointers to raw data representing the position, velocity and acceleration of a transmission'...
std::vector< double * > velocity
ActuatorToJointStateInterface act_to_jnt_state
RobotTransmissions * robot_transmissions
Lifecycle is externally controlled (ie. hardware abstraction)
std::vector< double * > position
std::vector< double * > effort
std::vector< ActuatorInfo > actuators_
std::vector< double * > effort
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
std::map< std::string, RawJointData > RawJointDataMap
std::vector< JointInfo > joints_
bool updateJointInterfaces(const TransmissionInfo &transmission_info, hardware_interface::RobotHW *robot_hw, JointInterfaces &joint_interfaces, RawJointDataMap &raw_joint_data_map) override
Update a robot's joint interfaces with joint information contained in a transmission.
Raw data for a set of joints.
ActuatorData act_state_data