joint_state_interface_provider.cpp
Go to the documentation of this file.
1 // Copyright (C) 2013, PAL Robotics S.L.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright notice,
7 // this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of PAL Robotics S.L. nor the names of its
12 // contributors may be used to endorse or promote products derived from
13 // this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 // POSSIBILITY OF SUCH DAMAGE.
27 
28 
29 // Pluginlib
31 
32 // ros_control
34 
35 namespace transmission_interface
36 {
37 
40  JointInterfaces& joint_interfaces,
41  RawJointDataMap& raw_joint_data_map)
42 {
43  // If interface does not yet exist in robot hardware abstraction, add it and use internal data structures
45  if (!robot_hw->get<JointStateInterface>())
46  {
47  robot_hw->registerInterface(&joint_interfaces.joint_state_interface);
48  }
49  JointStateInterface& interface = *(robot_hw->get<JointStateInterface>());
50 
51  // Register joints on the hardware interface
52  for (const auto& joint_info : transmission_info.joints_)
53  {
54  const std::string& name = joint_info.name_;
55 
56  // Do nothing if joint already exists on the hardware interface
57  if (hasResource(name, interface)) {continue;}
58 
59  // Update hardware interface
61  RawJointData& raw_joint_data = raw_joint_data_map[name]; // Add joint if it does not yet exist
62  if(raw_joint_data.hasAbsolutePosition && raw_joint_data.hasTorqueSensor)
63  {
64  JointStateHandle handle(name,
65  &raw_joint_data.position,
66  &raw_joint_data.velocity,
67  &raw_joint_data.effort,
68  &raw_joint_data.absolute_position,
69  &raw_joint_data.torque_sensor);
70  interface.registerHandle(handle);
71  }
72  else if(raw_joint_data.hasAbsolutePosition)
73  {
74  JointStateHandle handle(name,
75  &raw_joint_data.position,
76  &raw_joint_data.velocity,
77  &raw_joint_data.effort,
78  &raw_joint_data.absolute_position);
79  interface.registerHandle(handle);
80  }
81  else if(raw_joint_data.hasTorqueSensor)
82  {
83  JointStateHandle handle(name,
84  &raw_joint_data.position,
85  &raw_joint_data.velocity,
86  &raw_joint_data.effort,
87  &raw_joint_data.torque_sensor, true);
88  interface.registerHandle(handle);
89  }
90  else
91  {
92  JointStateHandle handle(name,
93  &raw_joint_data.position,
94  &raw_joint_data.velocity,
95  &raw_joint_data.effort);
96  interface.registerHandle(handle);
97  }
98 
99  }
100  return true;
101 }
102 
104  const RawJointDataMap& raw_joint_data_map,
105  JointData& jnt_state_data)
106 {
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);
111 
112  bool hasAbsolutePosition = true;
113  bool hasTorqueSensor = true;
114 
115  for (unsigned int i = 0; i < dim; ++i)
116  {
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;} // Joint name not found!
120  const RawJointData& raw_joint_data = raw_joint_data_it->second;
121 
122  hasAbsolutePosition = hasAbsolutePosition && raw_joint_data.hasAbsolutePosition;
123  hasTorqueSensor = hasTorqueSensor && raw_joint_data.hasTorqueSensor;
124  }
125 
126  if(hasAbsolutePosition)
127  {
128  jnt_state_data.absolute_position.resize(dim);
129  }
130 
131  if(hasTorqueSensor)
132  {
133  jnt_state_data.torque_sensor.resize(dim);
134  }
135 
136  for (unsigned int i = 0; i < dim; ++i)
137  {
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;} // Joint name not found!
141  const RawJointData& raw_joint_data = raw_joint_data_it->second;
142 
143  // TODO: Get rid of these const casts!
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)
148  {
149  jnt_state_data.absolute_position[i] = const_cast<double*>(&(raw_joint_data.absolute_position));
150  }
151  if(hasTorqueSensor)
152  {
153  jnt_state_data.torque_sensor[i] = const_cast<double*>(&(raw_joint_data.torque_sensor));
154  }
155  }
156 
157  return true;
158 }
159 
161  hardware_interface::RobotHW* robot_hw,
162  ActuatorData& act_state_data)
163 {
166 
167  // Get handles to all required actuators
168  std::vector<ActuatorStateHandle> handles;
169  if (!this->getActuatorHandles<ActuatorStateInterface, ActuatorStateHandle>(transmission_info.actuators_,
170  robot_hw,
171  handles)) {return false;}
172 
173  // Populate actuator data
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);
178 
179  bool hasAbsolutePositionInterface = true;
180  bool hasTorqueSensorInterface = true;
181 
182  for (unsigned int i = 0; i < dim; ++i)
183  {
184  hasAbsolutePositionInterface = hasAbsolutePositionInterface && handles[i].hasAbsolutePosition();
185  hasTorqueSensorInterface = hasTorqueSensorInterface && handles[i].hasTorqueSensor();
186  }
187 
188  if(hasAbsolutePositionInterface)
189  {
190  act_state_data.absolute_position.resize(dim);
191  }
192 
193  if(hasTorqueSensorInterface)
194  {
195  act_state_data.torque_sensor.resize(dim);
196  }
197 
198  for (unsigned int i = 0; i < dim; ++i)
199  {
200  // TODO: Get rid of these const casts!
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)
205  {
206  act_state_data.absolute_position[i] = const_cast<double*>(handles[i].getAbsolutePositionPtr());
207  }
208  if(hasTorqueSensorInterface)
209  {
210  act_state_data.torque_sensor[i] = const_cast<double*>(handles[i].getTorqueSensorPtr());
211  }
212  }
213  return true;
214 }
215 
217  TransmissionHandleData& handle_data)
218 {
219  // If interface does not yet exist in the robot transmissions, add it and use internal data structures
221  {
223  }
225 
226  // Update transmission interface
227  ActuatorToJointStateHandle handle(handle_data.name,
228  handle_data.transmission.get(),
229  handle_data.act_state_data,
230  handle_data.jnt_state_data);
231  interface.registerHandle(handle);
232  return true;
233 }
234 
235 } // namespace
236 
std::vector< double * > absolute_position
Definition: transmission.h:68
std::vector< double * > velocity
Definition: transmission.h:66
std::vector< double * > absolute_position
Definition: transmission.h:55
Contains pointers to raw data representing the position, velocity and acceleration of a transmission&#39;...
Definition: transmission.h:63
std::vector< double * > position
Definition: transmission.h:52
std::vector< double * > torque_sensor
Definition: transmission.h:69
Contains semantic info about a given transmission loaded from XML (URDF)
std::vector< double * > torque_sensor
Definition: transmission.h:56
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...
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&#39;...
Definition: transmission.h:50
std::vector< double * > velocity
Definition: transmission.h:53
RobotTransmissions * robot_transmissions
Lifecycle is externally controlled (ie. hardware abstraction)
std::vector< double * > position
Definition: transmission.h:65
std::vector< double * > effort
Definition: transmission.h:67
std::vector< ActuatorInfo > actuators_
std::vector< double * > effort
Definition: transmission.h:54
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
std::map< std::string, RawJointData > RawJointDataMap
bool updateJointInterfaces(const TransmissionInfo &transmission_info, hardware_interface::RobotHW *robot_hw, JointInterfaces &joint_interfaces, RawJointDataMap &raw_joint_data_map) override
Update a robot&#39;s joint interfaces with joint information contained in a transmission.


transmission_interface
Author(s): Adolfo Rodriguez Tsouroukdissian
autogenerated on Sun May 10 2020 03:14:56