Merge lp:~shadowrobot/sr-ros-interface-ethercat/tactiles_software_version into lp:sr-ros-interface-ethercat
- tactiles_software_version
- Merge into sr_hand_palm_edc
Proposed by
Ugo
Status: | Merged | ||||
---|---|---|---|---|---|
Merged at revision: | 521 | ||||
Proposed branch: | lp:~shadowrobot/sr-ros-interface-ethercat/tactiles_software_version | ||||
Merge into: | lp:sr-ros-interface-ethercat | ||||
Diff against target: |
298 lines (+132/-16) 6 files modified
sr_robot_lib/include/sr_robot_lib/tactile_sensors.hpp (+48/-5) sr_robot_lib/src/biotac.cpp (+2/-2) sr_robot_lib/src/generic_tactiles.cpp (+1/-1) sr_robot_lib/src/shadow_PSTs.cpp (+2/-2) sr_robot_lib/src/sr_robot_lib.cpp (+13/-2) sr_robot_lib/src/tactile_sensors.cpp (+66/-4) |
||||
To merge this branch: | bzr merge lp:~shadowrobot/sr-ros-interface-ethercat/tactiles_software_version | ||||
Related bugs: |
|
Reviewer | Review Type | Date Requested | Status |
---|---|---|---|
Hugo Elias (community) | Approve | ||
Review via email: mp+84764@code.launchpad.net |
Commit message
Description of the change
Fixed the software version for the tactiles.
To post a comment you must log in.
- 521. By Hand <hand@syntouch-big>
-
TESTED: Small corrections + documentation
Revision history for this message
Ugo (ugocupcic) wrote : | # |
Revision history for this message
Hugo Elias (hugo-shadowrobot) : | # |
review:
Approve
Preview Diff
[H/L] Next/Prev Comment, [J/K] Next/Prev File, [N/P] Next/Prev Hunk
1 | === modified file 'sr_robot_lib/include/sr_robot_lib/tactile_sensors.hpp' | |||
2 | --- sr_robot_lib/include/sr_robot_lib/tactile_sensors.hpp 2011-10-26 18:08:05 +0000 | |||
3 | +++ sr_robot_lib/include/sr_robot_lib/tactile_sensors.hpp 2011-12-07 14:07:26 +0000 | |||
4 | @@ -27,6 +27,9 @@ | |||
5 | 27 | #ifndef _TACTILE_SENSORS_HPP_ | 27 | #ifndef _TACTILE_SENSORS_HPP_ |
6 | 28 | #define _TACTILE_SENSORS_HPP_ | 28 | #define _TACTILE_SENSORS_HPP_ |
7 | 29 | 29 | ||
8 | 30 | #include <iostream> | ||
9 | 31 | #include <sstream> | ||
10 | 32 | #include <string> | ||
11 | 30 | #include <string> | 33 | #include <string> |
12 | 31 | #include <vector> | 34 | #include <vector> |
13 | 32 | 35 | ||
14 | @@ -41,11 +44,15 @@ | |||
15 | 41 | 44 | ||
16 | 42 | GenericTactileData(bool tactile_data_valid, int sample_frequency, | 45 | GenericTactileData(bool tactile_data_valid, int sample_frequency, |
17 | 43 | std::string manufacturer, std::string serial_number, | 46 | std::string manufacturer, std::string serial_number, |
19 | 44 | int software_version, int pcb_version) | 47 | int software_version_current, int software_version_server, |
20 | 48 | bool software_version_modified, int pcb_version) | ||
21 | 45 | : tactile_data_valid(tactile_data_valid), sample_frequency(sample_frequency), | 49 | : tactile_data_valid(tactile_data_valid), sample_frequency(sample_frequency), |
25 | 46 | manufacturer(manufacturer), serial_number(serial_number), | 50 | manufacturer(manufacturer), serial_number(serial_number), |
26 | 47 | software_version(software_version), pcb_version(pcb_version) | 51 | software_version_current(software_version_current), |
27 | 48 | {}; | 52 | software_version_server(software_version_server), |
28 | 53 | software_version_modified(software_version_modified), | ||
29 | 54 | pcb_version(pcb_version) | ||
30 | 55 | {}; | ||
31 | 49 | 56 | ||
32 | 50 | ~GenericTactileData() {}; | 57 | ~GenericTactileData() {}; |
33 | 51 | 58 | ||
34 | @@ -56,8 +63,36 @@ | |||
35 | 56 | std::string manufacturer; | 63 | std::string manufacturer; |
36 | 57 | std::string serial_number; | 64 | std::string serial_number; |
37 | 58 | 65 | ||
39 | 59 | int software_version; | 66 | int software_version_current; |
40 | 67 | int software_version_server; | ||
41 | 68 | bool software_version_modified; | ||
42 | 69 | |||
43 | 70 | /** | ||
44 | 71 | * Parses the version string received | ||
45 | 72 | * from the tactiles and fill in the | ||
46 | 73 | * variables. | ||
47 | 74 | * | ||
48 | 75 | * @version The raw version string. | ||
49 | 76 | */ | ||
50 | 77 | void set_software_version( std::string version ); | ||
51 | 78 | /** | ||
52 | 79 | * Formats the software version for the | ||
53 | 80 | * diagnostics. | ||
54 | 81 | * | ||
55 | 82 | * @return the formatted string | ||
56 | 83 | */ | ||
57 | 84 | virtual std::string get_software_version(); | ||
58 | 85 | |||
59 | 60 | int pcb_version; | 86 | int pcb_version; |
60 | 87 | |||
61 | 88 | inline double convertToInt(std::string const& s) | ||
62 | 89 | { | ||
63 | 90 | std::istringstream i(s); | ||
64 | 91 | int x; | ||
65 | 92 | if (!(i >> x)) | ||
66 | 93 | x = -1; | ||
67 | 94 | return x; | ||
68 | 95 | } | ||
69 | 61 | }; | 96 | }; |
70 | 62 | 97 | ||
71 | 63 | class PST3Data | 98 | class PST3Data |
72 | @@ -103,6 +138,14 @@ | |||
73 | 103 | int tac; //int16u in word[2] | 138 | int tac; //int16u in word[2] |
74 | 104 | int tdc; //int16u in word[2] | 139 | int tdc; //int16u in word[2] |
75 | 105 | boost::array<short int, 19ul> electrodes; //int16u in word[2] | 140 | boost::array<short int, 19ul> electrodes; //int16u in word[2] |
76 | 141 | |||
77 | 142 | /** | ||
78 | 143 | * Formats the software version for the | ||
79 | 144 | * diagnostics. | ||
80 | 145 | * | ||
81 | 146 | * @return the formatted string | ||
82 | 147 | */ | ||
83 | 148 | virtual std::string get_software_version(); | ||
84 | 106 | }; | 149 | }; |
85 | 107 | } | 150 | } |
86 | 108 | 151 | ||
87 | 109 | 152 | ||
88 | === modified file 'sr_robot_lib/src/biotac.cpp' | |||
89 | --- sr_robot_lib/src/biotac.cpp 2011-12-05 18:12:11 +0000 | |||
90 | +++ sr_robot_lib/src/biotac.cpp 2011-12-07 14:07:26 +0000 | |||
91 | @@ -192,7 +192,7 @@ | |||
92 | 192 | case TACTILE_SENSOR_TYPE_SOFTWARE_VERSION: | 192 | case TACTILE_SENSOR_TYPE_SOFTWARE_VERSION: |
93 | 193 | if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) ) | 193 | if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) ) |
94 | 194 | { | 194 | { |
96 | 195 | tactiles_vector->at(id_sensor).software_version = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[0]) ); | 195 | tactiles_vector->at(id_sensor).set_software_version( status_data->tactile[id_sensor].string ); |
97 | 196 | } | 196 | } |
98 | 197 | break; | 197 | break; |
99 | 198 | 198 | ||
100 | @@ -280,7 +280,7 @@ | |||
101 | 280 | d.addf("Manufacturer", "%s", tactiles_vector->at(id_tact).manufacturer.c_str()); | 280 | d.addf("Manufacturer", "%s", tactiles_vector->at(id_tact).manufacturer.c_str()); |
102 | 281 | d.addf("Serial Number", "%s", tactiles_vector->at(id_tact).serial_number.c_str()); | 281 | d.addf("Serial Number", "%s", tactiles_vector->at(id_tact).serial_number.c_str()); |
103 | 282 | 282 | ||
105 | 283 | d.addf("Software Version", "%d", tactiles_vector->at(id_tact).software_version); | 283 | d.addf("Software Version", "%s", tactiles_vector->at(id_tact).get_software_version().c_str()); |
106 | 284 | d.addf("PCB Version", "%d", tactiles_vector->at(id_tact).pcb_version); | 284 | d.addf("PCB Version", "%d", tactiles_vector->at(id_tact).pcb_version); |
107 | 285 | 285 | ||
108 | 286 | vec.push_back(d); | 286 | vec.push_back(d); |
109 | 287 | 287 | ||
110 | === modified file 'sr_robot_lib/src/generic_tactiles.cpp' | |||
111 | --- sr_robot_lib/src/generic_tactiles.cpp 2011-12-05 18:12:11 +0000 | |||
112 | +++ sr_robot_lib/src/generic_tactiles.cpp 2011-12-07 14:07:26 +0000 | |||
113 | @@ -102,7 +102,7 @@ | |||
114 | 102 | if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) ) | 102 | if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) ) |
115 | 103 | { | 103 | { |
116 | 104 | if( tactiles_vector != NULL ) | 104 | if( tactiles_vector != NULL ) |
118 | 105 | tactiles_vector->at(id_sensor).software_version = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[0]) ); | 105 | tactiles_vector->at(id_sensor).set_software_version( status_data->tactile[id_sensor].string ); |
119 | 106 | } | 106 | } |
120 | 107 | break; | 107 | break; |
121 | 108 | 108 | ||
122 | 109 | 109 | ||
123 | === modified file 'sr_robot_lib/src/shadow_PSTs.cpp' | |||
124 | --- sr_robot_lib/src/shadow_PSTs.cpp 2011-12-05 18:12:11 +0000 | |||
125 | +++ sr_robot_lib/src/shadow_PSTs.cpp 2011-12-07 14:07:26 +0000 | |||
126 | @@ -120,7 +120,7 @@ | |||
127 | 120 | case TACTILE_SENSOR_TYPE_SOFTWARE_VERSION: | 120 | case TACTILE_SENSOR_TYPE_SOFTWARE_VERSION: |
128 | 121 | if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) ) | 121 | if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) ) |
129 | 122 | { | 122 | { |
131 | 123 | tactiles_vector->at(id_sensor).software_version = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[0]) ); | 123 | tactiles_vector->at(id_sensor).set_software_version( status_data->tactile[id_sensor].string ); |
132 | 124 | } | 124 | } |
133 | 125 | break; | 125 | break; |
134 | 126 | 126 | ||
135 | @@ -187,7 +187,7 @@ | |||
136 | 187 | d.addf("Manufacturer", "%s", tactiles_vector->at(id_tact).manufacturer.c_str()); | 187 | d.addf("Manufacturer", "%s", tactiles_vector->at(id_tact).manufacturer.c_str()); |
137 | 188 | d.addf("Serial Number", "%s", tactiles_vector->at(id_tact).serial_number.c_str()); | 188 | d.addf("Serial Number", "%s", tactiles_vector->at(id_tact).serial_number.c_str()); |
138 | 189 | 189 | ||
140 | 190 | d.addf("Software Version", "%d", tactiles_vector->at(id_tact).software_version); | 190 | d.addf("Software Version", "%d", tactiles_vector->at(id_tact).get_software_version().c_str()); |
141 | 191 | d.addf("PCB Version", "%d", tactiles_vector->at(id_tact).pcb_version); | 191 | d.addf("PCB Version", "%d", tactiles_vector->at(id_tact).pcb_version); |
142 | 192 | 192 | ||
143 | 193 | d.addf("Pressure Raw", "%d", tactiles_vector->at(id_tact).pressure_raw); | 193 | d.addf("Pressure Raw", "%d", tactiles_vector->at(id_tact).pressure_raw); |
144 | 194 | 194 | ||
145 | === modified file 'sr_robot_lib/src/sr_robot_lib.cpp' | |||
146 | --- sr_robot_lib/src/sr_robot_lib.cpp 2011-12-06 18:43:27 +0000 | |||
147 | +++ sr_robot_lib/src/sr_robot_lib.cpp 2011-12-07 14:07:26 +0000 | |||
148 | @@ -606,7 +606,7 @@ | |||
149 | 606 | if( joint_tmp->motor->motor_id == 8 ) | 606 | if( joint_tmp->motor->motor_id == 8 ) |
150 | 607 | { | 607 | { |
151 | 608 | //ROS_ERROR_STREAM("Current " <<actuator->state_.last_measured_current_); | 608 | //ROS_ERROR_STREAM("Current " <<actuator->state_.last_measured_current_); |
153 | 609 | msg_debug.data = actuator->state_.last_measured_current_; | 609 | msg_debug.data = static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc); |
154 | 610 | debug_publishers[2].publish(msg_debug); | 610 | debug_publishers[2].publish(msg_debug); |
155 | 611 | } | 611 | } |
156 | 612 | #endif | 612 | #endif |
157 | @@ -619,7 +619,7 @@ | |||
158 | 619 | if( joint_tmp->motor->motor_id == 8 ) | 619 | if( joint_tmp->motor->motor_id == 8 ) |
159 | 620 | { | 620 | { |
160 | 621 | //ROS_ERROR_STREAM("Voltage " <<actuator->state_.motor_voltage_); | 621 | //ROS_ERROR_STREAM("Voltage " <<actuator->state_.motor_voltage_); |
162 | 622 | msg_debug.data = actuator->state_.motor_voltage_; | 622 | msg_debug.data = static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc); |
163 | 623 | debug_publishers[3].publish(msg_debug); | 623 | debug_publishers[3].publish(msg_debug); |
164 | 624 | } | 624 | } |
165 | 625 | #endif | 625 | #endif |
166 | @@ -745,9 +745,20 @@ | |||
167 | 745 | } | 745 | } |
168 | 746 | 746 | ||
169 | 747 | if (read_torque) | 747 | if (read_torque) |
170 | 748 | { | ||
171 | 748 | actuator->state_.last_measured_effort_ = | 749 | actuator->state_.last_measured_effort_ = |
172 | 749 | static_cast<double>(static_cast<int16s>(status_data->motor_data_packet[index_motor_in_msg].torque)); | 750 | static_cast<double>(static_cast<int16s>(status_data->motor_data_packet[index_motor_in_msg].torque)); |
173 | 750 | 751 | ||
174 | 752 | #ifdef DEBUG_PUBLISHER | ||
175 | 753 | if( joint_tmp->motor->motor_id == 8 ) | ||
176 | 754 | { | ||
177 | 755 | //ROS_ERROR_STREAM("Torque " << static_cast<int16s>(status_data->motor_data_packet[index_motor_in_msg].torque)); | ||
178 | 756 | msg_debug.data = static_cast<int16s>(status_data->motor_data_packet[index_motor_in_msg].torque); | ||
179 | 757 | debug_publishers[4].publish(msg_debug); | ||
180 | 758 | } | ||
181 | 759 | #endif | ||
182 | 760 | } | ||
183 | 761 | |||
184 | 751 | //Check the message to see if everything has already been received | 762 | //Check the message to see if everything has already been received |
185 | 752 | if (motor_current_state == operation_mode::device_update_state::INITIALIZATION) | 763 | if (motor_current_state == operation_mode::device_update_state::INITIALIZATION) |
186 | 753 | { | 764 | { |
187 | 754 | 765 | ||
188 | === modified file 'sr_robot_lib/src/tactile_sensors.cpp' | |||
189 | --- sr_robot_lib/src/tactile_sensors.cpp 2011-12-05 12:36:19 +0000 | |||
190 | +++ sr_robot_lib/src/tactile_sensors.cpp 2011-12-07 14:07:26 +0000 | |||
191 | @@ -25,10 +25,46 @@ | |||
192 | 25 | */ | 25 | */ |
193 | 26 | 26 | ||
194 | 27 | #include "sr_robot_lib/tactile_sensors.hpp" | 27 | #include "sr_robot_lib/tactile_sensors.hpp" |
195 | 28 | #include <boost/algorithm/string.hpp> | ||
196 | 29 | #include <boost/algorithm/string/find_iterator.hpp> | ||
197 | 30 | #include <sstream> | ||
198 | 28 | 31 | ||
199 | 32 | #include <ros/ros.h> | ||
200 | 29 | namespace tactiles | 33 | namespace tactiles |
201 | 30 | { | 34 | { |
202 | 31 | 35 | ||
203 | 36 | void GenericTactileData::set_software_version( std::string version ) | ||
204 | 37 | { | ||
205 | 38 | //split the string to fill the different versions | ||
206 | 39 | std::vector<std::string> splitted_string; | ||
207 | 40 | boost::split(splitted_string, version, boost::is_any_of("\n")); | ||
208 | 41 | |||
209 | 42 | BOOST_ASSERT(splitted_string.size() >= 3); | ||
210 | 43 | software_version_current = convertToInt(splitted_string[0]); | ||
211 | 44 | software_version_server = convertToInt(splitted_string[1]); | ||
212 | 45 | |||
213 | 46 | if( splitted_string[2] == "No") | ||
214 | 47 | software_version_modified = false; | ||
215 | 48 | else | ||
216 | 49 | software_version_modified = true; | ||
217 | 50 | } | ||
218 | 51 | |||
219 | 52 | std::string GenericTactileData::get_software_version() | ||
220 | 53 | { | ||
221 | 54 | //concatenate versions in a string. | ||
222 | 55 | std::string full_version; | ||
223 | 56 | |||
224 | 57 | std::stringstream ss; | ||
225 | 58 | if( software_version_modified ) | ||
226 | 59 | ss << "current: " << software_version_current << " / server: " << software_version_server << " / MODIFIED"; | ||
227 | 60 | else | ||
228 | 61 | ss << "current: " << software_version_current << " / server: " << software_version_server << " / not modified"; | ||
229 | 62 | |||
230 | 63 | full_version = ss.str(); | ||
231 | 64 | |||
232 | 65 | return full_version; | ||
233 | 66 | } | ||
234 | 67 | |||
235 | 32 | PST3Data::PST3Data() | 68 | PST3Data::PST3Data() |
236 | 33 | : GenericTactileData() | 69 | : GenericTactileData() |
237 | 34 | {}; | 70 | {}; |
238 | @@ -36,7 +72,10 @@ | |||
239 | 36 | PST3Data::PST3Data(const PST3Data& pst3) | 72 | PST3Data::PST3Data(const PST3Data& pst3) |
240 | 37 | : GenericTactileData(pst3.tactile_data_valid, pst3.sample_frequency, | 73 | : GenericTactileData(pst3.tactile_data_valid, pst3.sample_frequency, |
241 | 38 | pst3.manufacturer, pst3.serial_number, | 74 | pst3.manufacturer, pst3.serial_number, |
243 | 39 | pst3.software_version, pst3.pcb_version), | 75 | pst3.software_version_current, |
244 | 76 | pst3.software_version_server, | ||
245 | 77 | pst3.software_version_modified, | ||
246 | 78 | pst3.pcb_version), | ||
247 | 40 | pressure(pst3.pressure), temperature(pst3.temperature), | 79 | pressure(pst3.pressure), temperature(pst3.temperature), |
248 | 41 | debug_1(pst3.debug_1), debug_2(pst3.debug_2), | 80 | debug_1(pst3.debug_1), debug_2(pst3.debug_2), |
249 | 42 | pressure_raw(pst3.pressure_raw), zero_tracking(pst3.zero_tracking), dac_value(pst3.dac_value) | 81 | pressure_raw(pst3.pressure_raw), zero_tracking(pst3.zero_tracking), dac_value(pst3.dac_value) |
250 | @@ -45,7 +84,10 @@ | |||
251 | 45 | PST3Data::PST3Data(const GenericTactileData& gtd) | 84 | PST3Data::PST3Data(const GenericTactileData& gtd) |
252 | 46 | : GenericTactileData(gtd.tactile_data_valid, gtd.sample_frequency, | 85 | : GenericTactileData(gtd.tactile_data_valid, gtd.sample_frequency, |
253 | 47 | gtd.manufacturer, gtd.serial_number, | 86 | gtd.manufacturer, gtd.serial_number, |
255 | 48 | gtd.software_version, gtd.pcb_version) | 87 | gtd.software_version_current, |
256 | 88 | gtd.software_version_server, | ||
257 | 89 | gtd.software_version_modified, | ||
258 | 90 | gtd.pcb_version) | ||
259 | 49 | {}; | 91 | {}; |
260 | 50 | 92 | ||
261 | 51 | BiotacData::BiotacData() | 93 | BiotacData::BiotacData() |
262 | @@ -55,7 +97,10 @@ | |||
263 | 55 | BiotacData::BiotacData(const BiotacData& btac) | 97 | BiotacData::BiotacData(const BiotacData& btac) |
264 | 56 | : GenericTactileData(btac.tactile_data_valid, btac.sample_frequency, | 98 | : GenericTactileData(btac.tactile_data_valid, btac.sample_frequency, |
265 | 57 | btac.manufacturer, btac.serial_number, | 99 | btac.manufacturer, btac.serial_number, |
267 | 58 | btac.software_version, btac.pcb_version), | 100 | btac.software_version_current, |
268 | 101 | btac.software_version_server, | ||
269 | 102 | btac.software_version_modified, | ||
270 | 103 | btac.pcb_version), | ||
271 | 59 | pac0(btac.pac0), pac1(btac.pac1), | 104 | pac0(btac.pac0), pac1(btac.pac1), |
272 | 60 | pdc(btac.pdc), tac(btac.tac), | 105 | pdc(btac.pdc), tac(btac.tac), |
273 | 61 | tdc(btac.tdc) | 106 | tdc(btac.tdc) |
274 | @@ -69,8 +114,25 @@ | |||
275 | 69 | BiotacData::BiotacData(const GenericTactileData& gtd) | 114 | BiotacData::BiotacData(const GenericTactileData& gtd) |
276 | 70 | : GenericTactileData(gtd.tactile_data_valid, gtd.sample_frequency, | 115 | : GenericTactileData(gtd.tactile_data_valid, gtd.sample_frequency, |
277 | 71 | gtd.manufacturer, gtd.serial_number, | 116 | gtd.manufacturer, gtd.serial_number, |
279 | 72 | gtd.software_version, gtd.pcb_version) | 117 | gtd.software_version_current, |
280 | 118 | gtd.software_version_server, | ||
281 | 119 | gtd.software_version_modified, | ||
282 | 120 | gtd.pcb_version) | ||
283 | 73 | {}; | 121 | {}; |
284 | 122 | |||
285 | 123 | std::string BiotacData::get_software_version() | ||
286 | 124 | { | ||
287 | 125 | //concatenate versions in a string. | ||
288 | 126 | std::string full_version; | ||
289 | 127 | |||
290 | 128 | std::stringstream ss; | ||
291 | 129 | ss << "current: " << software_version_current; | ||
292 | 130 | |||
293 | 131 | full_version = ss.str(); | ||
294 | 132 | |||
295 | 133 | return full_version; | ||
296 | 134 | } | ||
297 | 135 | |||
298 | 74 | } | 136 | } |
299 | 75 | 137 | ||
300 | 76 | 138 |
this should now work.