45 #include <boost/asio.hpp>
46 #include <boost/array.hpp>
47 #include <boost/thread.hpp>
48 #include <boost/asio/ip/tcp.hpp>
49 #include <boost/thread/mutex.hpp>
50 #include <boost/thread/locks.hpp>
51 #include <boost/thread/thread.hpp>
55 #include <visualization_msgs/Marker.h>
56 #include <visualization_msgs/MarkerArray.h>
58 #include <Eigen/Dense>
59 #include <Eigen/Geometry>
61 #include <serialcom/BufferedAsyncSerial.h>
63 #include <opencv2/core/core.hpp>
64 #include <opencv2/highgui/highgui.hpp>
66 #include <sensor_msgs/Range.h>
67 #include <ros/package.h>
70 using namespace boost;
120 cv::FileStorage parameters(parameters_file.c_str(),cv::FileStorage::READ);
122 number_sensors = (int)parameters[
"number_sensors"];
124 cout<<
"number_sensors: "<<number_sensors<<endl;
125 for(uint s = 0;s<number_sensors;s++)
129 pos(0) = parameters[
"x"+to_string(s)];
130 pos(1) = parameters[
"y"+to_string(s)];
132 sensors_positions.push_back(pos);
134 Eigen::VectorXd curve(4);
136 curve(0) = parameters[
"p1_"+to_string(s)];
137 curve(1) = parameters[
"p2_"+to_string(s)];
138 curve(2) = parameters[
"p3_"+to_string(s)];
139 curve(3) = parameters[
"p4_"+to_string(s)];
141 sensors_curve_parameters.push_back(curve);
143 cout<<
"Reed parameters: "<<sensors_curve_parameters.size()<<endl;
144 pitch_bias = parameters[
"pitch_bias"];
145 roll_bias = parameters[
"roll_bias"];
168 Eigen::VectorXd curve_parameters = sensors_curve_parameters[sensor_id];
169 double a = curve_parameters(0);
170 double b = curve_parameters(1);
171 double c = curve_parameters(2);
172 double d = curve_parameters(3);
176 return a*exp(b*value)+c*exp(d*value);
261 serial(
"/dev/ttyACM0",115200)
264 outfile1.open((
"/home/carpinteiro/workingcopies/lar4/src/sensors/optoelectric/data/Sensors_bag.txt"));
268 string parameters_url;
270 nh_.param(
"parameters",parameters_url,std::string(
"no parameters file"));
272 string package_tag =
"package://";
273 int pos = parameters_url.find(package_tag);
274 if(pos != string::npos)
277 int fpos = parameters_url.find(
"/",pos+package_tag.length());
278 string package = parameters_url.substr(pos+package_tag.length(),fpos-(pos+package_tag.length()));
280 string package_path = ros::package::getPath(package);
281 parameters_url.replace(pos,fpos-pos,package_path);
284 cout <<
"url: " << parameters_url << endl;
286 s.readParameters(parameters_url);
300 for(uint i=0;i<8;i++)
302 ros::Publisher pub = nh_.advertise<sensor_msgs::Range>(
"sharp_"+to_string(i), 1000);
303 publishers.push_back(pub);
316 bool is_valid =
true;
321 if(msg.find(
"A0 ")==string::npos)
324 if(msg.find(
"A1 ")==string::npos)
327 if(msg.find(
"A2 ")==string::npos)
330 if(msg.find(
"A3 ")==string::npos)
333 if(msg.find(
"A4 ")==string::npos)
336 if(msg.find(
"A5 ")==string::npos)
339 if(msg.find(
"A6 ")==string::npos)
342 if(msg.find(
"A7 ")==string::npos)
360 string line = serial.readStringUntil(
"\n");
362 if(checkMessageIntegrety(line)==
false)
369 cout<<
"line: "<<endl;
371 cout<<
"size: "<<line.length()<<endl;
374 int values_reed = sscanf(line.c_str(),
"%*s %d %*s %d %*s %d %*s %d %*s %d %*s %d %*s %d %*s %d",&readings[0],&readings[1],&readings[2],&readings[3],&readings[4],&readings[5],&readings[6],&readings[7]);
376 cout <<
"leituras: " << readings[0] <<
" " <<readings[1] <<
" " <<readings[2] <<
" " <<readings[3] <<
" " <<readings[4] <<
" " <<readings[5] <<
" " <<readings[6] <<
" " <<readings[7] <<
" " << endl;
380 cout<<
"Failed to read all 8 values"<<endl;
381 cout<<
"Read: "<<values_reed<<endl;
385 vector<double> values;
386 for(
int i=0;i<values_reed;i++)
388 double voltage = s.convertoV(readings[i]);
389 double value = s.curveCalibration(i,voltage);
390 values.push_back(value);
394 for(uint i=0;i<values.size();i++)
396 sensor_msgs::Range msg;
398 msg.header.stamp = ros::Time::now();
399 msg.header.frame_id =
"sharp_"+to_string(i);
400 cout <<
"msg: " << msg.range << endl;
401 publishers[i].publish(msg);
404 outfile1<<ros::Time::now()<<
" ";
406 for(uint i=0;i<8;i++)
409 outfile1<<
" "<<values[i];
447 int main(
int argc,
char* argv[])
450 ros::init(argc, argv,
"orientation_base");
452 ros::NodeHandle nh(
"~");
SharpMath()
Class constructor.
OptoelectricVehiclePose(const ros::NodeHandle &nh)
Class constructor Init the serial port, open the txt file and the yaml file.
vector< Eigen::Vector2d > sensors_positions
void loop()
loop Function responsible to put the program running forever.
vector< Eigen::VectorXd > sensors_curve_parameters
double curveCalibration(uint sensor_id, double value)
curveCalibration Function responsible for convert the voltage signal to distance. Diferent sensors ha...
double convertoV(double A)
convertoV Function responsible for convert the analog signal to the equivalent voltage.
Plane()
Class constructor.
int main(int argc, char *argv[])
main Init ros and stays in loop forever.
vector< ros::Publisher > publishers
void readParameters(string parameters_file)
readParameters Function responsible for read the parameters from yaml file.
BufferedAsyncSerial serial
void setupMessaging()
setupMessaging Function responsible for creat the publishers.
bool checkMessageIntegrety(string msg)
checkMessageIntegrety Function responsible evaluate is the message is valid.