13 #include "std_msgs/String.h"
17 #include <trafeiro/fanuc_experiment.h>
19 #include <boost/array.hpp>
20 #include <boost/asio.hpp>
21 #include <boost/thread.hpp>
23 #include <serialcom/SerialCom.h>
28 using boost::asio::ip::tcp;
66 :io_service_(io_service),
69 tcp::endpoint endpoint = *endpoint_iterator;
78 strcpy(msg,
"GETCRCPOS\n");
80 write(msg,strlen(msg));
82 boost::asio::streambuf response;
83 istream response_stream(&response);
85 boost::asio::read_until(socket_, response,
"\n");
86 getline(response_stream, data);
88 boost::asio::read_until(socket_, response,
"\n");
89 getline(response_stream, data);
92 istringstream iss(data);
95 if (a==0) iss >> pos.
x;
96 if (a==1) iss >> pos.
y;
97 if (a==2) iss >> pos.
z;
101 boost::asio::read_until(socket_, response,
"\n");
102 getline(response_stream, data);
104 istringstream iss2(data);
108 if (a==0) iss2 >> pos.
w;
109 if (a==1) iss2 >> pos.
p;
110 if (a==2) iss2 >> pos.
r;
113 boost::asio::read_until(socket_, response,
"\n");
114 getline(response_stream, data);
117 boost::asio::read_until(socket_, response,
"\n");
118 getline(response_stream, data);
128 strcpy(msg,
"RUNTPP\n");
132 write(msg,strlen(msg));
134 boost::asio::streambuf response;
135 istream response_stream(&response);
137 boost::asio::read_until(socket_, response,
"\n");
138 getline(response_stream, data);
145 do_write(msg,length);
157 void handle_connect(
const boost::system::error_code& error,tcp::resolver::iterator endpoint_iterator)
162 }
else if (endpoint_iterator != tcp::resolver::iterator())
165 tcp::endpoint endpoint = *endpoint_iterator;
172 boost::asio::write(socket_,boost::asio::buffer(msg,length));
191 int main (
int argc ,
char** argv)
193 serialcom::SerialCom serial;
196 ros::init(argc , argv ,
"fanux_exp");
199 chatter_pub = n.advertise<trafeiro::fanuc_experiment>(
"topic_exp", 1000);
201 ros::Rate loop_rate(20);
206 std::string buffer_aux;
207 std::string Ax,Ay,Az,Mx,My,Mz,Gx,Gy,Gz,Y,P,R;
213 serial.open(
"/dev/ttyACM0",57600);
217 boost::asio::io_service io_service;
219 tcp::resolver resolver(io_service);
220 tcp::resolver::query query(
"192.168.0.230",
"4901");
222 tcp::resolver::iterator iterator = resolver.resolve(query);
226 trafeiro::fanuc_experiment experiment;
232 if (strcmp(argv[2],
"1")>=0)
236 if (strcmp(argv[2],
"0")>=0)
238 serial.write(
"#osct");
241 if ((strcmp(argv[2],
"1")<0) && (strcmp(argv[2],
"0")>0))
243 cout<<
"SENSOR output MODE invalid!"<<endl;
250 cout<<
"TPP NAME or SENSOR MODE MISSING ... TYPE fanuc_exp FILENAME SENSOR MODE"<<endl;
270 serial.readLine(buffer);
271 serial.readLine(buffer_aux);
274 std::basic_string <char> str2 = buffer.substr(5);
276 findPV = str2.find(
",");
277 std::basic_string <char> inf3 = str2.substr(findPV+1);
278 findSV = inf3.find(
",");
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());
299 serial.readLine(&sa,100);
300 serial.readLine(&sm,100);
301 serial.readLine(&sg,100);
302 }
catch(serialcom::Exception ex)
304 cout<<ex.what()<<endl;
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');
314 if((ncsa!=3 || ncsm!=3 || ncsg!=3) && (na==1 && nm==1 && ng==1))
323 istringstream ss(sa);
324 getline(ss,junk,
'=');
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());
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());
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());
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;
365 chatter_pub.publish(experiment);
373 }
catch (std::exception& e)
375 std::cerr <<
"Exception: " << e.what() <<
"\n";
void runtpp(char *tppname)
void do_write(char *msg, int length)
boost::asio::io_service & io_service_
void getcrcpos(Position &pos)
ros::Publisher chatter_pub
int main(int argc, char **argv)
CommunicationHandler(boost::asio::io_service &io_service, tcp::resolver::iterator endpoint_iterator)
void handle_connect(const boost::system::error_code &error, tcp::resolver::iterator endpoint_iterator)
void write(char *msg, int length)