pressure_cells.cpp
Go to the documentation of this file.
1 /**************************************************************************************************
2  Software License Agreement (BSD License)
3 
4  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
5  All rights reserved.
6 
7  Redistribution and use in source and binary forms, with or without modification, are permitted
8  provided that the following conditions are met:
9 
10  *Redistributions of source code must retain the above copyright notice, this list of
11  conditions and the following disclaimer.
12  *Redistributions in binary form must reproduce the above copyright notice, this list of
13  conditions and the following disclaimer in the documentation and/or other materials provided
14  with the distribution.
15  *Neither the name of the University of Aveiro nor the names of its contributors may be used to
16  endorse or promote products derived from this software without specific prior written permission.
17 
18  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
19  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
20  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
21  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
22  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
24  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
25  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 ***************************************************************************************************/
33 #include <ros/ros.h>
34 #include <std_msgs/String.h>
35 #include <visualization_msgs/Marker.h>
36 
37 #include <sys/types.h>
38 #include <unistd.h>
39 #include <stdlib.h>
40 
41 #include <sstream>
42 #include <unistd.h>
43 
44 #include <pressure_cells/arduino.h>
45 #include <pressure_cells/BufferedAsyncSerial.h>
46 #include <pressure_cells/SenVal.h>
47 
48 using namespace ros;
49 using namespace std;
50 
51 double ee_calibrate(double adc, int cell)
52 {
53 
54  if(cell==1)
55  {
56  //Calibrate cell I
57  return (double)(0.026574084375916 * adc -0.834411121568734);
58  }
59  if(cell==2)
60  {
61  //Calibrate cell II
62  return (double)(0.026233317081797 * adc -4.311235918674921);
63  }
64  if(cell==3)
65  {
66  //Calibrate cell III
67  return (double)(0.027158321888210 * adc -1.748472910437862);
68  }
69  if(cell==4)
70  {
71  //Calibrate cell IV
72  return (double)(0.026998009432107 * adc -3.485178371297848);
73  }
74  if(cell==5)
75  {
76  //Calibrate cell V
77  return (double)(0.061665094219161 * adc -0.037584380861199);
78  }
79  if(cell==6)
80  {
81  //Calibrate cell VI
82  return (double)(0.059355778053492 * adc -0.971028934842100);
83  }
84  if(cell==7)
85  {
86  //Calibrate cell VII
87  return (double)(0.061558158124062 * adc -1.514361056192554);
88  }
89  if(cell==8)
90  {
91  //Calibrate cell VIII
92  return (double)(0.062712489442863 * adc -0.845013447028478);
93  }
94 
95  return -1;
96 }
97 
102 int main ( int argc, char **argv )
103 {
104 
105 
106  bool viz=0;
107 
108  init( argc, argv, "arduino" );
109 
110  NodeHandle n("~");
111 
112  ros::Publisher pub_viz;
113 
114  string device;
115  string device_default = "default_device";
116  n.param("/device", device, device_default );
117 
118  // set marker publisher name
119  string tf_feet;
120  string tf_feet_default = "tf_default_feet";
121  n.param( "tf_feet", tf_feet, tf_feet_default );
122 
123  // frame for feet
124  string markers;
125  string markers_default = "default_markers";
126  n.param("markers", markers, markers_default );
127 
128 
129  std::string check_markers;
130  n.getParam("markers", check_markers);
131 
132  if (check_markers!=markers_default)
133  {
134  ROS_INFO("Setting markers parameter to %s", check_markers.c_str());
135  viz=1;
136  }
137 
138  // Cell Calibration
139 
140  int cell1;
141  int cell1_default = 1;
142  n.param("cell1", cell1, cell1_default );
143 
144  int cell2;
145  int cell2_default = 2;
146  n.param("cell2", cell2, cell2_default );
147 
148  int cell3;
149  int cell3_default = 3;
150  n.param("cell3", cell3, cell3_default );
151 
152  int cell4;
153  int cell4_default = 4;
154  n.param("cell4", cell4, cell4_default );
155 
156 
157  pub_viz = n.advertise<visualization_msgs::Marker>( markers, 0 );
158 
159  Publisher pub_sen_val = n.advertise< pressure_cells::SenVal >( "/values", 1000 );
160 
161  Rate loop_rate ( 1400 );
162 
163  pressure_cells::SenVal senval;
164 
165  BufferedAsyncSerial serial( device, 115200 );
166 
167  string data;
168  double adc1, adc2, adc3, adc4;
169 
170  int pid = getpid(), rpid;
171 
172  boost::format fmt("sudo renice -10 %d");
173  fmt % pid;
174 
175  rpid = std::system(fmt.str().c_str());
176 
177  while ( ok () )
178  {
179  try
180  {
181  // Always returns immediately. If the terminator \n has not yet
182  // arrived, returns an empty string.
183  data = serial.readStringUntil ( "\n" );
184 
185  //Incomplete message
186  if(data.size()!=5) {loop_rate.sleep ( ); continue;}
187 
188  adc1 = (unsigned char)data[0] << 2 | (unsigned char)data[4] >> 6 ;
189  adc2 = (unsigned char)data[1] << 2 | ((unsigned char)data[4] & 0x30) >> 4;
190  adc3 = (unsigned char)data[2] << 2 | ((unsigned char)data[4] & 0x0C) >> 2;
191  adc4 = (unsigned char)data[3] << 2 | ((unsigned char)data[4] & 0x03);
192 
193  senval.sen1=ee_calibrate(adc1, cell1); if(senval.sen1<=0) senval.sen1=0.001;
194  senval.sen2=ee_calibrate(adc2, cell2); if(senval.sen2<=0) senval.sen2=0.001;
195  senval.sen3=ee_calibrate(adc3, cell3); if(senval.sen3<=0) senval.sen3=0.001;
196  senval.sen4=ee_calibrate(adc4, cell4); if(senval.sen4<=0) senval.sen4=0.001;
197 
198  senval.header.stamp = ros::Time::now ( );
199 
200  pub_sen_val.publish ( senval );
201 
202  if(viz)
203  {
204  geometry_msgs::Point f1_p1;
205  geometry_msgs::Point f1_p2;
206  geometry_msgs::Point f2_p1;
207  geometry_msgs::Point f2_p2;
208  geometry_msgs::Point f3_p1;
209  geometry_msgs::Point f3_p2;
210  geometry_msgs::Point f4_p1;
211  geometry_msgs::Point f4_p2;
212 
213  visualization_msgs::Marker force1;
214  visualization_msgs::Marker force2;
215  visualization_msgs::Marker force3;
216  visualization_msgs::Marker force4;
217 
218  // Load Cell 1
219  force1.header.frame_id = tf_feet;
220  force1.header.stamp = ros::Time();
221  force1.ns = "force1";
222  force1.id = 0;
223  force1.type = visualization_msgs::Marker::ARROW;
224  force1.action = visualization_msgs::Marker::ADD;
225  force1.scale.x = 0.02;
226  force1.scale.y = 0.02;
227  force1.scale.z = 0;
228  force1.color.a = 1.0;
229  force1.color.r = 1.0;
230  force1.color.g = 0.5;
231  force1.color.b = 0.0;
232 
233  force1.points.clear();
234  f1_p1.x = 0.044;
235  f1_p1.y = 0.022;
236  f1_p1.z = 0;
237  force1.points.push_back(f1_p1);
238  f1_p2.x = 0.044;
239  f1_p2.y = 0.022;
240  f1_p2.z = senval.sen1/25;
241  force1.points.push_back(f1_p2);
242 
243  // Load Cell 2
244  force2.header.frame_id = tf_feet;
245  force2.header.stamp = ros::Time();
246  force2.ns = "force2";
247  force2.id = 0;
248  force2.type = visualization_msgs::Marker::ARROW;
249  force2.action = visualization_msgs::Marker::ADD;
250  force2.scale.x = 0.02;
251  force2.scale.y = 0.02;
252  force2.scale.z = 0;
253  force2.color.a = 1.0;
254  force2.color.r = 0.02;
255  force2.color.g = 0.8;
256  force2.color.b = 0.02;
257 
258  force2.points.clear();
259  f2_p1.x = 0.044;
260  f2_p1.y = -0.022;
261  f2_p1.z = 0;
262  force2.points.push_back(f2_p1);
263  f2_p2.x = 0.044;
264  f2_p2.y = -0.022;
265  f2_p2.z = senval.sen2/25;
266  force2.points.push_back(f2_p2);
267 
268  // Load Cell 3
269  force3.header.frame_id = tf_feet;
270  force3.header.stamp = ros::Time();
271  force3.ns = "force3";
272  force3.id = 0;
273  force3.type = visualization_msgs::Marker::ARROW;
274  force3.action = visualization_msgs::Marker::ADD;
275  force3.scale.x = 0.02;
276  force3.scale.y = 0.02;
277  force3.scale.z = 0;
278  force3.color.a = 1.0;
279  force3.color.r = 0.02;
280  force3.color.g = 0.37;
281  force3.color.b = 0.8;
282 
283  force3.points.clear();
284  f3_p1.x = -0.046;
285  f3_p1.y = -0.023;
286  f3_p1.z = 0;
287  force3.points.push_back(f3_p1);
288  f3_p2.x = -0.046;
289  f3_p2.y = -0.023;
290  f3_p2.z = senval.sen3/25;
291  force3.points.push_back(f3_p2);
292 
293  // Load Cell 4
294  force4.header.frame_id = tf_feet;
295  force4.header.stamp = ros::Time();
296  force4.ns = "force4";
297  force4.id = 0;
298  force4.type = visualization_msgs::Marker::ARROW;
299  force4.action = visualization_msgs::Marker::ADD;
300  force4.scale.x = 0.02;
301  force4.scale.y = 0.02;
302  force4.scale.z = 0;
303  force4.color.a = 1.0;
304  force4.color.r = 0.37;
305  force4.color.g = 0.02;
306  force4.color.b = 0.8;
307 
308  force4.points.clear();
309  f4_p1.x = -0.046;
310  f4_p1.y = 0.023;
311  f4_p1.z = 0;
312  force4.points.push_back(f4_p1);
313  f4_p2.x = -0.046;
314  f4_p2.y = 0.023;
315  f4_p2.z = senval.sen4/25;
316  force4.points.push_back(f4_p2);
317 
318  pub_viz.publish( force1 );
319  pub_viz.publish( force2 );
320  pub_viz.publish( force3 );
321  pub_viz.publish( force4 );
322 
323  }
324 
325  }
326  catch ( boost::system::system_error& e )
327  {
328  cout << "Error: " << e.what ( ) << endl;
329  return 1;
330  }
331 
332  spinOnce ( );
333 
334  loop_rate.sleep ( );
335 
336  }
337 
338  return 0;
339 }
ee_calibrate
double ee_calibrate(double adc, int cell)
Definition: pressure_cells.cpp:51
pub_viz
ros::Publisher pub_viz
Definition: force_cop.cpp:57
arduino.h
Header for communicating w/ the arduino.
BufferedAsyncSerial
Asynchronous serial class w/ buffer Asynchronous serial class that sends data to buffer after reading...
Definition: BufferedAsyncSerial.h:53
BufferedAsyncSerial::readStringUntil
std::string readStringUntil(const std::string delim="\n")
Definition: BufferedAsyncSerial.cpp:109
BufferedAsyncSerial.h
Class found to make buffers for Serial communications ()
main
int main(int argc, char **argv)
Definition: pressure_cells.cpp:102

pressure_cells
Author(s): Emilio Estrelinha
autogenerated on Mon Mar 2 2015 01:32:47