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);