rs232_tcp.cpp
Go to the documentation of this file.
1 
8 //TO LAUNCH THIS SCRIPT CORRETLY, must type "rosrun package fanuc_exp FILENAME S_MODE"
9 // S_MODE -> 1 to euler angles
10 // S_MODE -> 0 to raw values
11 
12 #include <ros/ros.h>
13 #include "std_msgs/String.h"
14 #include <sstream>
15 #include <string>
16 //INCLUDE OF MESSAGE STRUCTURE
17 #include <trafeiro/fanuc_experiment.h>
18 //INCLUDE BOOST LIBS
19 #include <boost/array.hpp>
20 #include <boost/asio.hpp>
21 #include <boost/thread.hpp>
22 //INCLUDE SERIAL LIB FROM LAR3/utils
23 #include <serialcom/SerialCom.h>
24 #include <algorithm>
25 
26 using namespace std;
27 
28 using boost::asio::ip::tcp;
33 class Position
34 {
35  public:
36  Position()
37  {
38  x=0;
39  y=0;
40  z=0;
41  w=0;
42  p=0;
43  r=0;
44  conf1=0;
45  conf2=0;
46  conf3=0;
47  turn1=0;
48  turn2=0;
49  turn3=0;
50  }
51 
52  double x,y,z;
53  double w,p,r;
54  int conf1,conf2,conf3;
55  int turn1,turn2,turn3;
56 };
57 
62 class CommunicationHandler
63 {
64  public:
65  CommunicationHandler(boost::asio::io_service& io_service,tcp::resolver::iterator endpoint_iterator)
66  :io_service_(io_service),
67  socket_(io_service)
68  {
69  tcp::endpoint endpoint = *endpoint_iterator;
70  socket_.async_connect(endpoint, boost::bind(&CommunicationHandler::handle_connect, this,boost::asio::placeholders::error, ++endpoint_iterator));
71  }
72 
73  void getcrcpos(Position& pos)
74  {
75  string data;
76 
77  char msg[1000];
78  strcpy(msg,"GETCRCPOS\n");
79 
80  write(msg,strlen(msg));
81 
82  boost::asio::streambuf response;
83  istream response_stream(&response);
84 
85  boost::asio::read_until(socket_, response, "\n");
86  getline(response_stream, data);//Current position
87 
88  boost::asio::read_until(socket_, response, "\n");
89  getline(response_stream, data);// X , Y , Z
90 
91  //STRING SPLITING
92  istringstream iss(data);
93  for(int a=0;a<3;a++)
94  {
95  if (a==0) iss >> pos.x;
96  if (a==1) iss >> pos.y;
97  if (a==2) iss >> pos.z;
98  }
99  cout<<data<<endl;
100 
101  boost::asio::read_until(socket_, response, "\n");
102  getline(response_stream, data);// W , P , R
103 
104  istringstream iss2(data);
105 
106  for(int a=0;a<3;a++)
107  {
108  if (a==0) iss2 >> pos.w;
109  if (a==1) iss2 >> pos.p;
110  if (a==2) iss2 >> pos.r;
111  }
112 
113  boost::asio::read_until(socket_, response, "\n");
114  getline(response_stream, data);// F , U , T
115 // cout<<data<<endl;
116 
117  boost::asio::read_until(socket_, response, "\n");
118  getline(response_stream, data);// REDUNDÂNCIAS
119 // cout<<data<<endl;
120  }
121 
122  void runtpp(char *tppname)
123  {
124  //PARA QUE ESTA FUNÇÃO FUNCIONE É PRECISO ENVIAR "1" a seguir ao nome da função
125  string data;
126  char msg[1000];
127 
128  strcpy(msg,"RUNTPP\n");
129  strcat(msg,tppname);
130  strcat(msg,"\n1\n");
131 
132  write(msg,strlen(msg));
133 
134  boost::asio::streambuf response;
135  istream response_stream(&response);
136 
137  boost::asio::read_until(socket_, response, "\n");
138  getline(response_stream, data);//On sucess it replies 1
139 
140 // cout<<data<<endl;
141  }
142 
143  void write(char* msg, int length)
144  {
145  do_write(msg,length);
146  }
147 
148  void close()
149  {
150  do_close();
151  }
152 
153 
154  private:
155  char data_[1000];
156 
157  void handle_connect(const boost::system::error_code& error,tcp::resolver::iterator endpoint_iterator)
158  {
159  if (!error)
160  {
161 
162  }else if (endpoint_iterator != tcp::resolver::iterator())
163  {
164  socket_.close();
165  tcp::endpoint endpoint = *endpoint_iterator;
166  socket_.async_connect(endpoint, boost::bind(&CommunicationHandler::handle_connect, this,boost::asio::placeholders::error, ++endpoint_iterator));
167  }
168  }
169 
170  void do_write(char*msg,int length)
171  {
172  boost::asio::write(socket_,boost::asio::buffer(msg,length));
173  }
174 
175  void do_close()
176  {
177  socket_.close();
178  }
179 
180  private:
181 
182  boost::asio::io_service& io_service_;
183  tcp::socket socket_;
184 
185 };
186 
191 int main (int argc ,char** argv)
192 {
193  serialcom::SerialCom serial;
194 
195 
196  ros::init(argc , argv , "fanux_exp");
197  ros::NodeHandle n;
198  ros::Publisher chatter_pub;
199  chatter_pub = n.advertise<trafeiro::fanuc_experiment>("topic_exp", 1000);
200 
201  ros::Rate loop_rate(20);
202 
203  try
204  {
205  std::string buffer;
206  std::string buffer_aux;
207  std::string Ax,Ay,Az,Mx,My,Mz,Gx,Gy,Gz,Y,P,R;
208  int MODE;
209  std::string junk;
210  Position p1;
211  int findPV, findSV;
212 
213  serial.open("/dev/ttyACM0",57600);
214 
215  usleep(3000000);
216 
217  boost::asio::io_service io_service;
218 
219  tcp::resolver resolver(io_service);
220  tcp::resolver::query query("192.168.0.230","4901");
221 
222  tcp::resolver::iterator iterator = resolver.resolve(query);
223 
224  CommunicationHandler c(io_service, iterator);
225 
226  trafeiro::fanuc_experiment experiment;
227 
228  char *tppname;
229 
230  if (argc>2)
231  {
232  if (strcmp(argv[2],"1")>=0)
233  {
234  MODE = 1;
235  }
236  if (strcmp(argv[2],"0")>=0)
237  {
238  serial.write("#osct");
239  MODE = 0;
240  }
241  if ((strcmp(argv[2],"1")<0) && (strcmp(argv[2],"0")>0))
242  {
243  cout<<"SENSOR output MODE invalid!"<<endl;
244  return 0;
245  }
246  tppname = argv[1];
247  }
248  else
249  {
250  cout<<"TPP NAME or SENSOR MODE MISSING ... TYPE fanuc_exp FILENAME SENSOR MODE"<<endl;
251  return 0;
252 // //once again,
253 // //TO LAUNCH THIS SCRIPT CORRETLY, it is necessary type "rosrun package fanuc_exp FILENAME S_MODE
254 // // S_MODE -> 1 to euler angles
255 // // S_MODE -> 0 to raw values
256  }
257  c.runtpp(tppname);
258  usleep(500000);
259 
260  // WHERE MAGIC HAPPENS!!!
261  // ////////////////////////////////////////////////////
262  // ////////////////////////////////////////////////////
263  while(ros::ok())
264  {
265 
266  if(MODE==1)
267  {
268  serial.write("#f");
269  usleep(5000);
270  serial.readLine(buffer);
271  serial.readLine(buffer_aux);//The sensor sends a additional empty line
272  if(buffer.size()>0)
273  {
274  std::basic_string <char> str2 = buffer.substr(5);
275 
276  findPV = str2.find(",");
277  std::basic_string <char> inf3 = str2.substr(findPV+1);
278  findSV = inf3.find(",");
279 
280  // WRITING SENSOR VALUES IN TOPIC VARIABLE
281  experiment.S_Y = (double) atof(str2.substr(0,findPV).c_str());
282  experiment.S_P = (double) atof(inf3.substr(0,findSV).c_str());
283  experiment.S_R = (double) atof(inf3.substr(findSV+1).c_str());
284  }
285  }
286  else
287  {
288 
289 
290  serial.flush();
291  serial.write("#f");
292 
293  string sa;
294  string sm;
295  string sg;
296 
297  try
298  {
299  serial.readLine(&sa,100);
300  serial.readLine(&sm,100);
301  serial.readLine(&sg,100);
302  }catch(serialcom::Exception ex)
303  {
304  cout<<ex.what()<<endl;
305  }
306 
307  int ncsa=count(sa.begin(), sa.end(), ',');
308  int ncsm=count(sm.begin(), sm.end(), ',');
309  int ncsg=count(sg.begin(), sg.end(), ',');
310  int na=count(sa.begin(), sa.end(), 'A');
311  int nm=count(sm.begin(), sm.end(), 'M');
312  int ng=count(sg.begin(), sg.end(), 'G');
313 
314  if((ncsa!=3 || ncsm!=3 || ncsg!=3) && (na==1 && nm==1 && ng==1))
315  {
316  exit(0);
317  }
318  else
319  {
320 
321  }
322 
323  istringstream ss(sa);
324  getline(ss,junk,'=');
325  getline(ss,Ax, ',');
326  getline(ss,Ay, ',');
327  getline(ss,Az, ',');
328  experiment.S_Ax = (double) atof(Ax.c_str());
329  experiment.S_Ay = (double) atof(Ay.c_str());
330  experiment.S_Az = (double) atof(Az.c_str());
331 
332 
333  istringstream ss2(sm);
334  getline(ss2,junk,'=');
335  getline(ss2,Mx, ',');
336  getline(ss2,My, ',');
337  getline(ss2,Mz, ',');
338  experiment.S_Mx = (double) atof(Mx.c_str());
339  experiment.S_My = (double) atof(My.c_str());
340  experiment.S_Mz = (double) atof(Mz.c_str());
341 
342 
343  istringstream ss3(sg);
344  getline(ss3,junk,'=');
345  getline(ss3,Gx, ',');
346  getline(ss3,Gy, ',');
347  getline(ss3,Gz, ',');
348  experiment.S_Gx = (double) atof(Gx.c_str());
349  experiment.S_Gy = (double) atof(Gy.c_str());
350  experiment.S_Gz = (double) atof(Gz.c_str());
351 
352 
353  }
354 // // //GETTING FANUC'S POSITION
355  c.getcrcpos(p1);
356 
357 // WRITING FANUC VALUES IN TOPIC VARIABLE
358  experiment.F_x = p1.x;
359  experiment.F_y = p1.y;
360  experiment.F_z = p1.z;
361  experiment.F_w = p1.w;
362  experiment.F_r = p1.r;
363  experiment.F_p = p1.p;
364 
365  chatter_pub.publish(experiment);
366 
367  loop_rate.sleep();
368 
369  }
370 
371  c.close();
372  serial.close();
373  }catch (std::exception& e)
374  {
375  std::cerr << "Exception: " << e.what() << "\n";
376  }
377 
378  return 0;
379 
380 }
CommunicationHandler::runtpp
void runtpp(char *tppname)
Definition: rs232_tcp.cpp:122
CommunicationHandler::do_write
void do_write(char *msg, int length)
Definition: rs232_tcp.cpp:170
Position::x
double x
Definition: rs232_tcp.cpp:52
CommunicationHandler::io_service_
boost::asio::io_service & io_service_
Definition: rs232_tcp.cpp:182
CommunicationHandler::getcrcpos
void getcrcpos(Position &pos)
Definition: rs232_tcp.cpp:73
chatter_pub
ros::Publisher chatter_pub
Definition: complementary_filter.cpp:57
Position::conf3
int conf3
Definition: rs232_tcp.cpp:54
Position::p
double p
Definition: rs232_tcp.cpp:53
Position::w
double w
Definition: rs232_tcp.cpp:53
main
int main(int argc, char **argv)
Definition: rs232_tcp.cpp:191
CommunicationHandler::CommunicationHandler
CommunicationHandler(boost::asio::io_service &io_service, tcp::resolver::iterator endpoint_iterator)
Definition: rs232_tcp.cpp:65
Position::turn3
int turn3
Definition: rs232_tcp.cpp:55
Position::y
double y
Definition: rs232_tcp.cpp:52
CommunicationHandler::handle_connect
void handle_connect(const boost::system::error_code &error, tcp::resolver::iterator endpoint_iterator)
Definition: rs232_tcp.cpp:157
CommunicationHandler::write
void write(char *msg, int length)
Definition: rs232_tcp.cpp:143
CommunicationHandler::socket_
tcp::socket socket_
Definition: rs232_tcp.cpp:183
Position::r
double r
Definition: rs232_tcp.cpp:53
n
ros::NodeHandle * n
Definition: complementary_filter.cpp:56
Position::z
double z
Definition: rs232_tcp.cpp:52

imu_network
Author(s): Telmo Rafeiro
autogenerated on Mon Mar 2 2015 01:31:44