35 #include "std_msgs/String.h"
41 #include <imu_network/raw_imu_message.h>
42 #include <imu_network/sensors_values.h>
43 #include <imu_network/sensors_network.h>
46 #include <serialcom/SerialCom.h>
58 int main (
int argc ,
char** argv)
61 serialcom::SerialCom serial;
63 ros::init(argc , argv ,
"accel_network_calibration");
66 ros::Rate loop_rate(10);
72 string Ax,Ay,Az,Mx,My,Mz,Gx,Gy,Gz;
74 int number,total_number;
78 serial.open(
"/dev/ttyACM0",115200);
85 serial.readLine(&number_buffer,100);
86 if ((count(number_buffer.begin(),number_buffer.end(),
'N')) && (count(number_buffer.begin(),number_buffer.end(),
',')) && (number_buffer.length()==3))
89 istringstream ssN(number_buffer);
90 getline(ssN,junk,
'N');
91 getline(ssN,converter,
',');
92 total_number = (int) atoi(converter.c_str());
93 ROS_INFO(
"Comunicação iniciada com %d sensores",total_number);
99 for(
int i=0;i<total_number;i++)
113 string data[total_number];
114 int ncsa[total_number];
115 int ncsm[total_number];
116 int ncsg[total_number];
117 int na[total_number];
118 int nm[total_number];
119 int ng[total_number];
121 string num[total_number];
129 for (
int c = 0; c<total_number;c++)
133 serial.readLine(&data[c],100);
135 }
catch(serialcom::Exception ex)
138 cout<<ex.what()<<endl;
141 ncsa[c]=count(data[c].begin(), data[c].end(),
',');
143 na[c]=count(data[c].begin(), data[c].end(),
'A');
144 nm[c]=count(data[c].begin(), data[c].end(),
'M');
145 ng[c]=count(data[c].begin(), data[c].end(),
'G');
157 istringstream ss(data[c]);
158 getline(ss,junk,
'A');
160 if(c == (
int)atoi(num[c].c_str()))
166 getline(ss,junk,
'M');
170 getline(ss,junk,
'G');
177 if (((
double) atof(Ax.c_str())) >
accel_max[0][c])
accel_max[0][c] = (
double) atof(Ax.c_str());
178 if (((
double) atof(Ay.c_str())) >
accel_max[1][c])
accel_max[1][c] = (
double) atof(Ay.c_str());
179 if (((
double) atof(Az.c_str())) >
accel_max[2][c])
accel_max[2][c] = (
double) atof(Az.c_str());
181 if (((
double) atof(Ax.c_str())) <
accel_min[0][c])
accel_min[0][c] = (
double) atof(Ax.c_str());
182 if (((
double) atof(Ay.c_str())) <
accel_min[1][c])
accel_min[1][c] = (
double) atof(Ay.c_str());
183 if (((
double) atof(Az.c_str())) <
accel_min[2][c])
accel_min[2][c] = (
double) atof(Az.c_str());
221 }
catch (std::exception& e)
223 std::cerr <<
"Exception: " << e.what() <<
"\n";
int main(int argc, char **argv)
vector< float > accel_min[3]
vector< float > accel_max[3]