Line data Source code
1 : /*
2 : * Copyright (C) 2019 IIT-HHCM
3 : * Author: Luca Muratore
4 : * email: luca.muratore@iit.it
5 : *
6 : * Licensed under the Apache License, Version 2.0 (the "License");
7 : * you may not use this file except in compliance with the License.
8 : * You may obtain a copy of the License at
9 : * http://www.apache.org/licenses/LICENSE-2.0
10 : *
11 : * Unless required by applicable law or agreed to in writing, software
12 : * distributed under the License is distributed on an "AS IS" BASIS,
13 : * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 : * See the License for the specific language governing permissions and
15 : * limitations under the License.
16 : */
17 :
18 : #include <ros_end_effector/HAL/EEHal.h>
19 :
20 41 : ROSEE::EEHal::EEHal(ros::NodeHandle* nh) {
21 :
22 41 : _nh = nh;
23 :
24 : //init sub to receive reference from UniversalROSEEEX
25 : //TODO take topic name from roslaunch
26 82 : std::string motor_reference_topic = "/ros_end_effector/motor_reference_pos";
27 :
28 82 : _motor_reference_sub = _nh->subscribe(motor_reference_topic, 10,
29 41 : &ROSEE::EEHal::motor_reference_clbk, this);
30 :
31 82 : std::string joint_state_topic = "/ros_end_effector/joint_states";
32 :
33 41 : _joint_state_pub = _nh->advertise<sensor_msgs::JointState>(joint_state_topic, 10);
34 :
35 41 : _hand_info_present = parseHandInfo();
36 :
37 41 : if (_hand_info_present) {
38 0 : _hand_info_service_name = "hand_info";
39 : }
40 :
41 41 : _pressure_active = false; // if a derived class want to use this, it must call initPressureSensing()
42 :
43 :
44 :
45 41 : }
46 :
47 22393 : bool ROSEE::EEHal::checkCommandPub() {
48 :
49 22393 : return (_motor_reference_sub.getNumPublishers() > 0 && _mr_msg.name.size() != 0);
50 : }
51 :
52 41 : bool ROSEE::EEHal::isHandInfoPresent() { return _hand_info_present; }
53 :
54 20367 : void ROSEE::EEHal::motor_reference_clbk(const sensor_msgs::JointState::ConstPtr& msg) {
55 :
56 20367 : _mr_msg = *msg;
57 :
58 20367 : }
59 :
60 22393 : bool ROSEE::EEHal::publish_joint_state() {
61 :
62 : //NOTE _js_msg must be filled by the derived class
63 22393 : _joint_state_pub.publish(_js_msg);
64 :
65 22393 : return true;
66 :
67 : }
68 :
69 0 : bool ROSEE::EEHal::getFingersNames(std::vector<std::string> &fingers_names){
70 :
71 0 : if (this->fingers_names.size() != 0) {
72 :
73 0 : fingers_names = this->fingers_names;
74 0 : return true;
75 :
76 : } else {
77 0 : return false;
78 : }
79 :
80 : }
81 0 : bool ROSEE::EEHal::getMotorsNames(std::vector<std::string> &motors_names){
82 :
83 0 : if (this->motors_names.size() != 0) {
84 :
85 0 : motors_names = this->motors_names;
86 0 : return true;
87 :
88 : } else {
89 0 : return false;
90 : }
91 :
92 : }
93 :
94 0 : bool ROSEE::EEHal::getMotorStiffness(Eigen::VectorXd &motors_stiffness){
95 :
96 0 : if (this->motors_stiffness.size() != 0) {
97 :
98 0 : motors_stiffness = this->motors_stiffness;
99 0 : return true;
100 :
101 : } else {
102 0 : return false;
103 : }
104 :
105 : }
106 :
107 0 : bool ROSEE::EEHal::getTipsFrictions(Eigen::VectorXd &tips_frictions){
108 :
109 0 : if (this->tips_frictions.size() != 0) {
110 :
111 0 : tips_frictions = this->tips_frictions;
112 0 : return true;
113 :
114 : } else {
115 0 : return false;
116 : }
117 :
118 : }
119 :
120 0 : bool ROSEE::EEHal::getMotorTorqueLimits(Eigen::VectorXd &motors_torque_limits){
121 :
122 0 : if (this->motors_torque_limits.size() != 0) {
123 :
124 0 : motors_torque_limits = this->motors_torque_limits;
125 0 : return true;
126 :
127 : } else {
128 0 : return false;
129 : }
130 :
131 : }
132 :
133 0 : bool ROSEE::EEHal::getTipJointToTipFrameX(Eigen::VectorXd &tip_joint_to_tip_frame_x) {
134 :
135 0 : if (this->tip_joint_to_tip_frame_x.size() != 0) {
136 :
137 0 : tip_joint_to_tip_frame_x = this->tip_joint_to_tip_frame_x;
138 0 : return true;
139 :
140 : } else {
141 0 : return false;
142 : }
143 : }
144 :
145 0 : bool ROSEE::EEHal::getTipJointToTipFrameY(Eigen::VectorXd &tip_joint_to_tip_frame_y) {
146 :
147 0 : if (this->tip_joint_to_tip_frame_y.size() != 0) {
148 :
149 0 : tip_joint_to_tip_frame_y = this->tip_joint_to_tip_frame_y;
150 0 : return true;
151 :
152 : } else {
153 0 : return false;
154 : }
155 : }
156 :
157 41 : bool ROSEE::EEHal::parseHandInfo() {
158 :
159 82 : std::string _rosee_config_path;
160 41 : if (! _nh->getParam ( "/ros_ee_config_path", _rosee_config_path )) {
161 0 : return false;
162 : }
163 :
164 82 : std::ifstream ifile(_rosee_config_path);
165 41 : if (! ifile) {
166 0 : ROS_WARN_STREAM ("EEHALExecutor: config file " << _rosee_config_path << " not found");
167 0 : return false;
168 : }
169 :
170 82 : YAML::Node node = YAML::LoadFile(_rosee_config_path);
171 :
172 41 : if (! node["hand_info"]) {
173 41 : ROS_WARN_STREAM ("EEHALExecutor: config file " << _rosee_config_path << " does not contain "
174 : << "hand_info node. I will not parse hand information");
175 :
176 41 : return false;
177 : }
178 :
179 0 : if(node["hand_info"]["fingers_names"]){
180 :
181 0 : fingers_names = node["hand_info"]["fingers_names"].as<std::vector<std::string>>();
182 : }
183 :
184 0 : if(node["hand_info"]["motors_names"]){
185 :
186 0 : motors_names = node["hand_info"]["motors_names"].as<std::vector<std::string>>();
187 : }
188 :
189 0 : if(node["hand_info"]["motors_stiffness"]){
190 :
191 0 : motors_stiffness = ROSEE::Utils::yamlVectorToEigen(node["hand_info"]["motors_stiffness"]);
192 : }
193 :
194 0 : if(node["hand_info"]["tips_frictions"]){
195 :
196 0 : tips_frictions = ROSEE::Utils::yamlVectorToEigen(node["hand_info"]["tips_frictions"]);
197 : }
198 :
199 0 : if(node["hand_info"]["motors_torque_limits"]){
200 :
201 0 : motors_torque_limits = ROSEE::Utils::yamlVectorToEigen(node["hand_info"]["motors_torque_limits"]);
202 : }
203 :
204 0 : if(node["hand_info"]["tip_joint_to_tip_frame_x"]){
205 :
206 0 : tip_joint_to_tip_frame_x = ROSEE::Utils::yamlVectorToEigen(node["hand_info"]["tip_joint_to_tip_frame_x"]);
207 : }
208 :
209 0 : if(node["hand_info"]["tip_joint_to_tip_frame_y"]){
210 :
211 0 : tip_joint_to_tip_frame_y = ROSEE::Utils::yamlVectorToEigen(node["hand_info"]["tip_joint_to_tip_frame_y"]);
212 : }
213 :
214 0 : return true;
215 : }
216 :
217 0 : bool ROSEE::EEHal::init_hand_info_response() {
218 :
219 0 : _hand_info_response.fingers_names = fingers_names;
220 :
221 0 : _hand_info_response.motors_names = motors_names;
222 :
223 0 : _hand_info_response.motors_stiffness =
224 0 : ROSEE::Utils::eigenVectorToStdVector(motors_stiffness);
225 :
226 0 : _hand_info_response.tips_frictions =
227 0 : ROSEE::Utils::eigenVectorToStdVector(tips_frictions);
228 :
229 0 : _hand_info_response.motors_torque_limits =
230 0 : ROSEE::Utils::eigenVectorToStdVector(motors_torque_limits);
231 :
232 0 : return true;
233 : }
234 :
235 0 : bool ROSEE::EEHal::setHandInfoCallback() {
236 :
237 0 : _hand_info_server = _nh->advertiseService(_hand_info_service_name,
238 0 : &ROSEE::EEHal::handInfoEEHalCallback, this);
239 0 : return true;
240 : }
241 :
242 0 : bool ROSEE::EEHal::handInfoEEHalCallback (
243 : rosee_msg::HandInfo::Request& request,
244 : rosee_msg::HandInfo::Response& response) {
245 :
246 : //generic hal does not read the request
247 :
248 0 : response = _hand_info_response;
249 :
250 0 : return true;
251 : }
252 :
253 0 : bool ROSEE::EEHal::initPressureSensing()
254 : {
255 :
256 0 : std::string topic_name = "/ros_end_effector/pressure_phalanges";
257 :
258 0 : _pressure_pub = _nh->advertise<rosee_msg::MotorPhalangePressure>(topic_name, 10);
259 :
260 0 : _pressure_active = true;
261 :
262 0 : return true;
263 : }
264 :
265 0 : bool ROSEE::EEHal::publish_pressure() {
266 :
267 : //NOTE _pressure_msg must be filled by the derived class
268 0 : _pressure_pub.publish(_pressure_msg);
269 :
270 0 : return true;
271 :
272 : }
273 :
274 0 : Eigen::VectorXd ROSEE::EEHal::getMotorReference() const {
275 :
276 0 : Eigen::VectorXd motorRef;
277 0 : motorRef.resize(_mr_msg.name.size());
278 0 : for (int i=0; i<_mr_msg.name.size(); i++ ) {
279 :
280 0 : motorRef(i) = _mr_msg.position.at(i);
281 : }
282 :
283 0 : return motorRef;
284 : }
285 :
286 0 : Eigen::VectorXd ROSEE::EEHal::getJointPosition() const {
287 :
288 0 : Eigen::VectorXd jointPos;
289 0 : jointPos.resize(_js_msg.name.size());
290 0 : for (int i=0; i<_js_msg.name.size(); i++ ) {
291 :
292 0 : jointPos(i) = _js_msg.position.at(i);
293 : }
294 :
295 0 : return jointPos;
296 : }
297 :
298 0 : Eigen::VectorXd ROSEE::EEHal::getJointEffort() const {
299 :
300 0 : Eigen::VectorXd jointEffort;
301 0 : jointEffort.resize(_js_msg.name.size());
302 0 : for (int i=0; i<_js_msg.name.size(); i++ ) {
303 :
304 0 : jointEffort(i) = _js_msg.effort.at(i);
305 : }
306 :
307 0 : return jointEffort;
308 : }
309 :
310 0 : Eigen::MatrixXd ROSEE::EEHal::getPressure() const {
311 :
312 0 : Eigen::MatrixXd pressure;
313 0 : pressure.resize(4, _pressure_msg.pressure_finger1.size()); //message has 4 finger field
314 0 : for (int i=0; i<_pressure_msg.pressure_finger1.size(); i++ ) {
315 :
316 0 : pressure(0, i) = _pressure_msg.pressure_finger1.at(i);
317 0 : pressure(1, i) = _pressure_msg.pressure_finger2.at(i);
318 0 : pressure(2, i) = _pressure_msg.pressure_finger3.at(i);
319 0 : pressure(3, i) = _pressure_msg.pressure_thumb.at(i);
320 : }
321 :
322 0 : return pressure;
323 :
324 123 : }
|
