effort_joint_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  // Setup joint state interface first
45  robot_hw,
46  joint_interfaces,
47  raw_joint_data_map)) {return false;}
48 
49  // If interface does not yet exist in robot hardware abstraction, add it and use internal data structures
51  if (!robot_hw->get<EffortJointInterface>())
52  {
53  robot_hw->registerInterface(&joint_interfaces.effort_joint_interface);
54  }
55  EffortJointInterface& interface = *(robot_hw->get<EffortJointInterface>());
56 
57  // Register joints on the hardware interface
58  BOOST_FOREACH(const JointInfo& joint_info, transmission_info.joints_)
59  {
60  const std::string& name = joint_info.name_;
61 
62  // Do nothing if joint already exists on the hardware interface
63  if (hasResource(name, interface)) {continue;}
64 
65  // Update hardware interface
67  RawJointData& raw_joint_data = raw_joint_data_map[name];
68  JointHandle handle(joint_interfaces.joint_state_interface.getHandle(joint_info.name_),
69  &raw_joint_data.effort_cmd);
70  interface.registerHandle(handle);
71  }
72  return true;
73 }
74 
76  const RawJointDataMap& raw_joint_data_map,
77  JointData& jnt_cmd_data)
78 {
79  const unsigned int dim = transmission_info.joints_.size();
80  jnt_cmd_data.effort.resize(dim);
81 
82  for (unsigned int i = 0; i < dim; ++i)
83  {
84  const std::string& joint_name = transmission_info.joints_[i].name_;
85  RawJointDataMap::const_iterator raw_joint_data_it = raw_joint_data_map.find(joint_name);
86  if (raw_joint_data_it == raw_joint_data_map.end()) {return false;} // Joint name not found!
87  const RawJointData& raw_joint_data = raw_joint_data_it->second;
88 
89  // TODO: Get rid of these const casts!
90  jnt_cmd_data.effort[i] = const_cast<double*>(&(raw_joint_data.effort_cmd));
91  }
92 
93  return true;
94 }
95 
98  ActuatorData& act_cmd_data)
99 {
102 
103  // Get handles to all required actuators
104  std::vector<ActuatorHandle> handles;
105  if (!this->getActuatorHandles<EffortActuatorInterface, ActuatorHandle>(transmission_info.actuators_,
106  robot_hw,
107  handles)) {return false;}
108 
109  // Populate actuator data
110  const unsigned int dim = transmission_info.actuators_.size();
111  act_cmd_data.effort.resize(dim);
112 
113  for (unsigned int i = 0; i < dim; ++i)
114  {
115  // TODO: Get rid of these const casts!
116  act_cmd_data.effort[i] = const_cast<double*>(handles[i].getCommandPtr());
117  }
118  return true;
119 }
120 
122  TransmissionHandleData& handle_data)
123 {
124  // Setup joint state interface first (if not yet done)
125  if (!hasResource(handle_data.name, loader_data.transmission_interfaces.act_to_jnt_state))
126  {
127  if (!JointStateInterfaceProvider::registerTransmission(loader_data, handle_data)) {return false;}
128  }
129 
130  // If command interface does not yet exist in the robot transmissions, add it and use internal data structures
132  {
134  }
136 
137  // Setup command interface
138  JointToActuatorEffortHandle handle(handle_data.name,
139  handle_data.transmission.get(),
140  handle_data.act_cmd_data,
141  handle_data.jnt_cmd_data);
142  interface.registerHandle(handle);
143  return true;
144 }
145 
146 } // namespace
147 
bool registerTransmission(TransmissionLoaderData &loader_data, TransmissionHandleData &handle_data)
Contains pointers to raw data representing the position, velocity and acceleration of a transmission&#39;...
Definition: transmission.h:61
bool updateJointInterfaces(const TransmissionInfo &transmission_info, hardware_interface::RobotHW *robot_hw, JointInterfaces &joint_interfaces, RawJointDataMap &raw_joint_data_map)
Update a robot&#39;s joint interfaces with joint information contained in a transmission.
Contains semantic info about a given joint loaded from XML (URDF)
Contains semantic info about a given transmission loaded from XML (URDF)
bool registerTransmission(TransmissionLoaderData &loader_data, TransmissionHandleData &handle_data)
bool getJointCommandData(const TransmissionInfo &transmission_info, const RawJointDataMap &raw_joint_data_map, JointData &jnt_cmd_data)
bool updateJointInterfaces(const TransmissionInfo &transmission_info, hardware_interface::RobotHW *robot_hw, JointInterfaces &joint_interfaces, RawJointDataMap &raw_joint_data_map)
Update a robot&#39;s joint interfaces with joint information contained in a transmission.
hardware_interface::EffortJointInterface effort_joint_interface
Joint interfaces of a robot. Only used interfaces need to be populated.
Handle for propagating joint efforts to actuator efforts for a given transmission.
static bool hasResource(const std::string &name, const Interface &iface)
hardware_interface::JointStateInterface joint_state_interface
bool getActuatorCommandData(const TransmissionInfo &transmission_info, hardware_interface::RobotHW *robot_hw, ActuatorData &act_cmd_data)
Contains pointers to raw data representing the position, velocity and acceleration of a transmission&#39;...
Definition: transmission.h:50
JointStateHandle getHandle(const std::string &name)
RobotTransmissions * robot_transmissions
Lifecycle is externally controlled (ie. hardware abstraction)
std::vector< double * > effort
Definition: transmission.h:65
std::vector< ActuatorInfo > actuators_
PLUGINLIB_EXPORT_CLASS(transmission_interface::BiDirectionalEffortJointInterfaceProvider, transmission_interface::RequisiteProvider)
std::vector< double * > effort
Definition: transmission.h:54
std::map< std::string, RawJointData > RawJointDataMap


transmission_interface
Author(s): Adolfo Rodriguez Tsouroukdissian
autogenerated on Fri Jun 7 2019 22:00:17