34 #include "std_msgs/String.h"
39 #include <imu_network/raw_imu_message.h>
40 #include <imu_network/sensors_values.h>
41 #include <imu_network/sensors_network.h>
44 #include <serialcom/SerialCom.h>
53 int main (
int argc ,
char** argv)
56 serialcom::SerialCom serial;
58 ros::init(argc , argv ,
"sensors_raw_data");
61 chatter_pub = n.advertise<imu_network::sensors_network>(
"topic_raw_data", 1000);
63 ros::Rate loop_rate(10);
70 string Ax,Ay,Az,Mx,My,Mz,Gx,Gy,Gz;
72 int number,total_number;
76 serial.open(
"/dev/ttyACM0",115200);
80 imu_network::sensors_network values;
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++)
101 imu_network::sensors_values sensor;
102 values.sensors_val.push_back(sensor);
109 string data[total_number];
110 int ncsa[total_number];
111 int ncsm[total_number];
112 int ncsg[total_number];
113 int na[total_number];
114 int nm[total_number];
115 int ng[total_number];
117 string num[total_number];
124 for (
int c = 0; c<total_number;c++)
128 serial.readLine(&data[c],100);
130 }
catch(serialcom::Exception ex)
133 cout<<ex.what()<<endl;
136 ncsa[c]=count(data[c].begin(), data[c].end(),
',');
138 na[c]=count(data[c].begin(), data[c].end(),
'A');
139 nm[c]=count(data[c].begin(), data[c].end(),
'M');
140 ng[c]=count(data[c].begin(), data[c].end(),
'G');
152 istringstream ss(data[c]);
153 getline(ss,junk,
'A');
155 if(c == (
int)atoi(num[c].c_str()))
161 getline(ss,junk,
'M');
165 getline(ss,junk,
'G');
172 values.sensors_val[c].S_Ax= (double) atof(Ax.c_str());
173 values.sensors_val[c].S_Ay = (double) atof(Ay.c_str());
174 values.sensors_val[c].S_Az = (double) atof(Az.c_str());
176 values.sensors_val[c].S_Mx = (double) atof(Mx.c_str());
177 values.sensors_val[c].S_My = (double) atof(My.c_str());
178 values.sensors_val[c].S_Mz = (double) atof(Mz.c_str());
180 values.sensors_val[c].S_Gx =(double)atof(Gx.c_str());
181 values.sensors_val[c].S_Gy =(double)atof(Gy.c_str());
182 values.sensors_val[c].S_Gz =(double)atof(Gz.c_str());
184 values.sensors_val[c].total_number = total_number;
185 values.sensors_val[c].header.stamp = ros::Time::now();
186 values.sensors_val[c].number = (int)atoi(num[c].c_str());
187 values.sensors_val[c].header.frame_id =
"imu_raw";
190 chatter_pub.publish(values);
196 }
catch (std::exception& e)
198 std::cerr <<
"Exception: " << e.what() <<
"\n";
int main(int argc, char **argv)
ros::Publisher chatter_pub