45 if (!robot_hw->
get<JointStateInterface>())
49 JointStateInterface&
interface = *(robot_hw->
get<JointStateInterface>());
54 const std::string& name = joint_info.
name_;
62 JointStateHandle handle(name,
66 interface.registerHandle(handle);
75 const unsigned int dim = transmission_info.
joints_.size();
78 jnt_state_data.
effort.resize(dim);
80 for (
unsigned int i = 0; i < dim; ++i)
82 const std::string& joint_name = transmission_info.
joints_[i].name_;
83 RawJointDataMap::const_iterator raw_joint_data_it = raw_joint_data_map.find(joint_name);
84 if (raw_joint_data_it == raw_joint_data_map.end()) {
return false;}
85 const RawJointData& raw_joint_data = raw_joint_data_it->second;
88 jnt_state_data.
position[i] =
const_cast<double*
>(&(raw_joint_data.
position));
89 jnt_state_data.
velocity[i] =
const_cast<double*
>(&(raw_joint_data.
velocity));
90 jnt_state_data.
effort[i] =
const_cast<double*
>(&(raw_joint_data.
effort));
104 std::vector<ActuatorStateHandle> handles;
105 if (!this->getActuatorHandles<ActuatorStateInterface, ActuatorStateHandle>(transmission_info.
actuators_,
107 handles)) {
return false;}
110 const unsigned int dim = transmission_info.
actuators_.size();
111 act_state_data.
position.resize(dim);
112 act_state_data.
velocity.resize(dim);
113 act_state_data.
effort.resize(dim);
115 for (
unsigned int i = 0; i < dim; ++i)
118 act_state_data.
position[i] =
const_cast<double*
>(handles[i].getPositionPtr());
119 act_state_data.
velocity[i] =
const_cast<double*
>(handles[i].getVelocityPtr());
120 act_state_data.
effort[i] =
const_cast<double*
>(handles[i].getEffortPtr());
140 interface.registerHandle(handle);
void registerInterface(T *iface)
TransmissionSharedPtr transmission
bool registerTransmission(TransmissionLoaderData &loader_data, TransmissionHandleData &handle_data)
std::vector< double * > velocity
bool getJointStateData(const TransmissionInfo &transmission_info, const RawJointDataMap &raw_joint_data_map, JointData &jnt_state_data)
Contains pointers to raw data representing the position, velocity and acceleration of a transmission'...
std::vector< double * > position
Contains semantic info about a given joint loaded from XML (URDF)
Contains semantic info about a given transmission loaded from XML (URDF)
bool updateJointInterfaces(const TransmissionInfo &transmission_info, hardware_interface::RobotHW *robot_hw, JointInterfaces &joint_interfaces, RawJointDataMap &raw_joint_data_map)
Update a robot's joint interfaces with joint information contained in a transmission.
Joint interfaces of a robot. Only used interfaces need to be populated.
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)
hardware_interface::JointStateInterface joint_state_interface
bool getActuatorStateData(const TransmissionInfo &transmission_info, hardware_interface::RobotHW *robot_hw, ActuatorData &act_state_data)
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_
PLUGINLIB_EXPORT_CLASS(transmission_interface::BiDirectionalEffortJointInterfaceProvider, transmission_interface::RequisiteProvider)
std::vector< double * > effort
std::map< std::string, RawJointData > RawJointDataMap
std::vector< JointInfo > joints_
Raw data for a set of joints.
ActuatorData act_state_data