serial_com.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  * Copyright (c) 2013, Intelligent Robotics Lab, DLUT.
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *     * Neither the name of the Willow Garage, Inc. and Intelligent Robotics 
00015  *        Lab, DLUT nor the names of its contributors may be used to endorse or
00016  *       promote products derived from this software without specific prior written 
00017  *       permission.
00018  *
00019  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00020  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00021  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00022  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00023  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00024  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00025  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00026  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00027  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00028  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029  * POSSIBILITY OF SUCH DAMAGE.
00030 *********************************************************************/
00031 
00032 #include "ros/ros.h"
00033 #include "std_msgs/String.h"
00034 #include "dlut_move_base/Velocity.h"
00035 #include "dlut_move_base/Twist.h"
00036 
00037 //the serial port
00038 #include <stdio.h>
00039 #include <string.h>
00040 #include <sys/types.h>
00041 #include <errno.h>
00042 #include <sys/stat.h>
00043 #include <fcntl.h>
00044 #include <unistd.h>
00045 #include <termios.h>
00046 #include <stdlib.h>
00047 
00048 #define DEFAULT_PORT 2
00049 #define DEFAULT_BAUDRATE 9600
00050 
00051 #include <tf/transform_broadcaster.h>
00052 #include <nav_msgs/Odometry.h>
00053 
00054 #include <iostream>
00055 #include <fstream>
00056 
00057 class SerialCom
00058 {
00059 public:
00060   SerialCom();
00061   ~SerialCom();
00062   int openPort(int fd, int comport);
00063   int setOpt(int fd,int nspeed,int nbits,char nevent,int nstop);
00064   int packSend(int fd,char ptr,char buff[]);
00065   int packGet(int fd,char ptr);
00066   void comCallBack(const dlut_move_base::Velocity &vel);//send the control cmd to the port by Velocity
00067   void velSend(const dlut_move_base::Twist &tw);//send the control cmd to the port by Twist
00068   int comInit();
00069  
00070   dlut_move_base::Twist twist_;//the velocity we get from the robot.
00071   void timerPackGet(const ros::TimerEvent &);
00072   ros::NodeHandle serial_node_handle_;
00073 
00074 private:
00075   int fd_;//the file discriptor of the serial port
00076 
00077   ros::Subscriber sub_cmd_vel_,sub_cmd_twist_; 
00078   ros::Publisher vel_pub_;//get the v and w from the serial of the robot.
00079   ros::Publisher odom_pub_;//output the odom of the robot by v and w got from the serial
00080 
00081   tf::TransformBroadcaster odom_broadcaster_;//publish the odom transform
00082 
00083   int port_,baudrate_;
00084 
00085   //the initial pose of the robot
00086   double x_;
00087   double y_;
00088   double th_;
00089 
00090   //the v and w of the robot
00091   double vx_;
00092   double vy_;
00093   double vth_;
00094 
00095   ros::Time current_time_,last_time_;
00096 };
00097 
00098 SerialCom::SerialCom()
00099 {
00100   sub_cmd_vel_ = serial_node_handle_.subscribe("cmd_velocity", 1000, &SerialCom::comCallBack,this);
00101   sub_cmd_twist_ = serial_node_handle_.subscribe("cmd_twist", 1000, &SerialCom::velSend,this);
00102   vel_pub_ = serial_node_handle_.advertise<dlut_move_base::Twist>("robot_velocity", 1);
00103 
00104   serial_node_handle_.param("Port",port_,DEFAULT_PORT);
00105   serial_node_handle_.param("BaudRate",baudrate_,DEFAULT_BAUDRATE);
00106 
00107   twist_.angular.z=0.0;
00108   twist_.linear.x=0.0;
00109 
00110   comInit();
00111  
00112   x_ = 0.0;
00113   y_ = 0.0;
00114   th_ = 0.0;
00115 
00116   vx_ = 0.0;
00117   vy_ = 0.0;
00118   vth_ = 0.0;
00119 
00120   current_time_ = ros::Time::now();
00121   last_time_ = ros::Time::now();
00122   
00123   odom_pub_ = serial_node_handle_.advertise<nav_msgs::Odometry>("odom", 50);
00124 }
00125 
00126 SerialCom::~SerialCom()
00127 {
00128   close(fd_);
00129 }
00130 
00131 //open the serial port
00132 int SerialCom::openPort(int fd, int comport)
00133 {
00134   if(comport == 1)//port 1
00135   {
00136     fd = open("/dev/ttyS0",O_RDWR|O_NOCTTY|O_NDELAY);
00137     
00138     if(-1  ==  fd)
00139     {
00140       ROS_INFO("failed to open port 1!");
00141       return(-1);
00142     }
00143   }
00144   else if(comport == 2)//port 2
00145   {
00146     fd = open("/dev/ttyS1",O_RDWR|O_NOCTTY|O_NDELAY);
00147     if(-1 == fd)
00148     {
00149       ROS_INFO("failed to open port 2!");
00150       return(-1);
00151     }
00152   }
00153   else if(comport == 3)//port 3
00154   {
00155     fd = open("/dev/ttyS2",O_RDWR|O_NOCTTY|O_NDELAY);
00156         
00157     if(-1 == fd)
00158     {
00159       ROS_INFO("failed to open port 3!");
00160       return(-1);
00161     }
00162   }
00163 
00164   if(fcntl(fd,F_SETFL,0)<0)
00165     ROS_INFO("fcntl failed!");
00166   else
00167     ROS_INFO("fcntl=%d.",fcntl(fd,F_SETFL,0));
00168 
00169   if(isatty(STDIN_FILENO) == 0)
00170     ROS_INFO("standsrd input is not a terminal device.");
00171   else
00172     ROS_INFO("is a tty sucess.");
00173 
00174   ROS_INFO("the return value of the serial open function=%d,!=-1,indicates succeed to open the serial.",fd);
00175         
00176   return fd;
00177 }
00178 
00179 //setup the serial port
00180 int SerialCom::setOpt(int fd,int nspeed,int nbits,char nevent,int nstop)
00181 {
00182   struct termios newtio,oldtio;
00183 
00184   //check the parameter of the serial port to see whether there is error or not
00185   if(tcgetattr(fd,&oldtio) != 0)
00186   {
00187     ROS_INFO("failed to setup the serial,failed to save the serial value!");
00188     return -1;  
00189   }
00190 
00191   bzero(&newtio,sizeof(newtio));
00192 
00193   newtio.c_cflag |= CLOCAL | CREAD;
00194   newtio.c_cflag &= ~CSIZE;
00195 
00196   switch(nbits)
00197   {
00198     case 7:
00199       newtio.c_cflag |= CS7;
00200       break;
00201     case 8:
00202       newtio.c_cflag |= CS8;
00203       break;    
00204   }
00205 
00206   switch(nevent)
00207   {
00208     case 'O': 
00209       newtio.c_cflag |= PARENB;
00210       newtio.c_cflag |= PARODD;
00211       newtio.c_iflag |= (INPCK | ISTRIP);
00212       break;
00213     case 'E':
00214       newtio.c_iflag |= (INPCK | ISTRIP);
00215       newtio.c_cflag |= PARENB ;
00216       newtio.c_cflag &= ~ PARODD;
00217       break;
00218     case 'N':
00219       newtio.c_cflag &= ~ PARENB;
00220       break;            
00221   }
00222 
00223   //setup the baud rate
00224   switch(nspeed)
00225   {
00226     case 2400:
00227       cfsetispeed(&newtio,B2400);
00228       cfsetospeed(&newtio,B2400);
00229       break;            
00230     case 4800:
00231       cfsetispeed(&newtio,B4800);
00232       cfsetospeed(&newtio,B4800);
00233       break;    
00234     case 9600:
00235       cfsetispeed(&newtio,B9600);
00236       cfsetospeed(&newtio,B9600);
00237       break;    
00238     case 19200:
00239       cfsetispeed(&newtio,B19200);
00240       cfsetospeed(&newtio,B19200);
00241       break;    
00242     case 38400:
00243       cfsetispeed(&newtio,B38400);
00244       cfsetospeed(&newtio,B38400);
00245       break;
00246     case 57600:
00247       cfsetispeed(&newtio,B57600);
00248       cfsetospeed(&newtio,B57600);
00249       break;            
00250     case 115200:
00251       cfsetispeed(&newtio,B115200);
00252       cfsetospeed(&newtio,B115200);
00253       break;    
00254     case 460800:
00255       cfsetispeed(&newtio,B460800);
00256       cfsetospeed(&newtio,B460800);
00257       break;    
00258     default:
00259       cfsetispeed(&newtio,B9600);
00260       cfsetospeed(&newtio,B9600);
00261       break;            
00262   }
00263 
00264   //setup the stop bit
00265   if(nstop == 1)
00266     newtio.c_cflag &= ~ CSTOPB;
00267   else if(nstop == 2)
00268     newtio.c_cflag |= CSTOPB;
00269         
00270   //setup the waitting time and the minimum amount of the characters received 
00271   newtio.c_cc[VTIME] = 0;
00272   newtio.c_cc[VMIN] = 0;
00273 
00274   //deal with the characters which were not received.
00275   tcflush(fd,TCIFLUSH);
00276 
00277   //activate the new serial setup
00278   if((tcsetattr(fd,TCSANOW,&newtio))!=0)
00279   {
00280     ROS_INFO("failed to activate the new serial setup!");
00281     return -1;
00282   }
00283         
00284   ROS_INFO("serial setup success!\n");
00285 
00286   return 0;
00287 }
00288 /*
00289 a pack of data consists of four parts:
00290 1.the start sign: "*"
00291 2.the data sign: 
00292   "l":the velocity of the left wheel
00293   "r":the velocity of the right wheel
00294   "v":the velocity of the robot
00295   "w":the angular velocity of the robot
00296 3.the length of the data:
00297 4.the data
00298 */
00299 int SerialCom::packSend(int fd,char ptr,char buff[])
00300 {
00301   int nwrite;   
00302 
00303   //write the start sign
00304   char begin[2];
00305   begin[0] = '*';
00306   nwrite = write(fd,begin,1);
00307   begin[nwrite] = '\0';
00308  
00309   //write the data sign
00310   char sign[2];//the sign of the pack,'l'、'r'、'v' or 'w'
00311   if(ptr == 'l' || ptr == 'r' || ptr == 'v' || ptr == 'w')
00312   {
00313     sign[0] = ptr;
00314     sign[1] = '\0';
00315     nwrite = write(fd,sign,1);          
00316   }
00317   else
00318   {
00319     ROS_INFO("wrong parameter!");
00320     return 0;
00321   }
00322 
00323   //write the length of the data
00324   char len[2];
00325   len[0] = strlen(buff)+'0';
00326   len[1] = '\0';
00327   if(buff[0] != '-')//if the data is minus,add 1 to the length
00328   {
00329     len[0]++;
00330   }
00331   nwrite = write(fd,len,1);
00332   ROS_INFO("len[0]=%d.\n",len[0]);
00333 
00334   //write the data to the port
00335   if(buff[0] != '-')
00336   {
00337     nwrite = write(fd,"+",1);//写串口
00338   }
00339   nwrite = write(fd,buff,strlen(buff));//写串口
00340   buff[nwrite] = '\0';
00341   
00342   return 0;
00343 }
00344 
00345 /*
00346 get a pack of data which contains v and w of the robot from the serial port
00347 */
00348 int SerialCom::packGet(int fd,char ptr)
00349 {
00350   int nwrite,nread;
00351   char buff[10]="\0";
00352   if(ptr == 'v')
00353   {
00354     nwrite=write(fd,"?v",2);
00355   }
00356   else if(ptr == 'w')
00357   {
00358     nwrite=write(fd,"?w",2);  
00359   }
00360   else if(ptr == 'l')
00361   {
00362     nwrite=write(fd,"?l",2); 
00363   }
00364   else if(ptr == 'r')
00365   {
00366     nwrite=write(fd,"?r",2);    
00367   }
00368   else if(ptr == 'a')
00369   {
00370     nwrite=write(fd,"?v",2);
00371     nwrite=write(fd,"?w",2);
00372     nwrite=write(fd,"?l",2);
00373     nwrite=write(fd,"?r",2);    
00374   }
00375   else
00376   {
00377     ROS_INFO("the parameter of the function packGet must be ‘v’、‘w’、‘l’、‘r’ or 'all'!");
00378     return false;
00379   }
00380   
00381   if(nwrite < 0)
00382   {
00383     ROS_INFO("send data error!");     
00384   }
00385   
00386   while(1)
00387   {
00388     if((nread = read(fd,buff,8)) == 8)
00389     {
00390       buff[8]='\0';
00391       if(buff[0] != '*' || buff[2] != '5')
00392       {
00393         ROS_INFO("The pack we get from the robot isn't right!(the start sign or the length of the data is not right!)");
00394       }
00395       else if(buff[1] == 'v' || buff[1] == 'w' || buff[1] == 'l' || buff[1] == 'r')
00396       {
00397         switch(buff[1])
00398         {
00399           case 'v':
00400             ROS_INFO("the velocity of the robot V=");
00401             twist_.linear.x=atof(&buff[3]);
00402             break;
00403           case 'w':
00404             ROS_INFO("the angle velocity of the robot W=");
00405             twist_.angular.z=atof(&buff[3]);
00406             break;
00407           case 'l':
00408             ROS_INFO("the velocity of the left wheel=");
00409             break;
00410           case 'r':
00411             ROS_INFO("the velocity of the right wheel=");
00412             break;
00413           default:
00414             ROS_INFO("the sign is wrong!");     
00415         }
00416         
00417         ROS_INFO("%s.",&buff[3]);       
00418         break;                          
00419       }
00420     }
00421   }
00422 
00423   return true;
00424 }
00425 
00426 void SerialCom::comCallBack(const dlut_move_base::Velocity &vel)
00427 {
00428   char pl[10];
00429   char pa[10];
00430         
00431   ROS_INFO("linear: [%f] angular: [%f].",vel.linear,vel.angular);       
00432   ROS_INFO("linear: [%s] angular: [%s].",gcvt(vel.linear,6,pl),gcvt(vel.angular,6,pa));
00433 
00434   packSend(fd_,'v',pl);//send the value of the velocity
00435   packSend(fd_,'w',pa);//send the value of the angular velocity
00436 }
00437 
00438 void SerialCom::velSend(const dlut_move_base::Twist &tw)
00439 {
00440   char pl[10];
00441   char pa[10];  
00442 
00443   double vel_linear,vel_angular;
00444   vel_linear = tw.linear.x;
00445   vel_angular = tw.angular.z;
00446 
00447   ROS_INFO("linear: [%f] angular: [%f].",vel_linear,vel_angular);
00448 
00449   ROS_INFO("linear: [%s].",gcvt(vel_linear,6,pl));
00450   ROS_INFO("angular: [%s].",gcvt(vel_angular,6,pa));
00451 
00452   packSend(fd_,'v',pl);//send the value of the velocity
00453   packSend(fd_,'w',pa);//send the value of the angular velocity
00454   ROS_INFO("velSend function called!");
00455 }
00456 
00457 int SerialCom::comInit()
00458 {
00459   int i;
00460 
00461   if((fd_ = openPort(fd_,port_)) < 0)
00462   {
00463     ROS_INFO("failed to setup the serial!");
00464     return 0;
00465   }
00466 
00467   if((i = setOpt(fd_,baudrate_,8,'N',1)) < 0)
00468   {
00469     ROS_INFO("failed to setup the serial!");
00470     return 0;
00471   }
00472 
00473   ROS_INFO("the serial openned,setup the serial successed。the file operator=%d",fd_); 
00474   
00475   return 0;
00476 }
00477 void SerialCom::timerPackGet(const ros::TimerEvent &)
00478 {
00479   ROS_INFO("timerPackGet function called!");    
00480   packGet(fd_,'v');//get the value of the velocity from the serial port
00481   packGet(fd_,'w');//get the value of the angular velocity from the serial port 
00482   vel_pub_.publish(twist_);//in fact,we can delete this line,but we can publish the v and w if not.
00483 
00484   vx_ = twist_.linear.x;
00485   vy_ = 0.0;
00486   vth_ = twist_.angular.z;
00487   ros::Time current_time_ = ros::Time::now();
00488 
00489   double dt = (current_time_ - last_time_).toSec();
00490   double delta_x = (vx_ * cos(th_) - vy_ * sin(th_)) * dt;
00491   double delta_y = (vx_ * sin(th_) + vy_ * cos(th_)) * dt;
00492   double delta_th = vth_ * dt;
00493 
00494   x_ += delta_x;
00495   y_ += delta_y;
00496   th_ += delta_th;
00497 
00498   geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th_);
00499 
00500   geometry_msgs::TransformStamped odom_trans;
00501   odom_trans.header.stamp = current_time_;
00502   odom_trans.header.frame_id = "odom";
00503   odom_trans.child_frame_id = "base_link";
00504 
00505   odom_trans.transform.translation.x = x_;
00506   odom_trans.transform.translation.y = y_;
00507   odom_trans.transform.translation.z = 0.0;
00508   odom_trans.transform.rotation = odom_quat;
00509 
00510   odom_broadcaster_.sendTransform(odom_trans);
00511   //odom_broadcasterPtr->sendTransform(odom_trans);
00512 
00513   nav_msgs::Odometry odom;
00514   odom.header.stamp = current_time_;
00515   odom.header.frame_id = "odom";
00516 
00517   odom.pose.pose.position.x = x_;
00518   odom.pose.pose.position.y = y_;
00519   odom.pose.pose.position.z = 0.0;
00520   odom.pose.pose.orientation = odom_quat;
00521 
00522   odom.child_frame_id = "base_link";
00523   odom.twist.twist.linear.x = vx_;
00524   odom.twist.twist.linear.y = vy_;
00525   odom.twist.twist.angular.z = vth_;
00526 
00527   ROS_INFO("the pose of the robot =%f,%f,%f.",x_,y_,th_);
00528   ROS_INFO("the v and of the robot =%f.",vx_);
00529   ROS_INFO("the w and of the robot =%f.",vth_);
00530        
00531   odom_pub_.publish(odom);
00532 
00533   last_time_ = current_time_;
00534 }
00535 
00536 int main(int argc, char **argv)
00537 {
00538   ros::init(argc, argv, "serial_com");
00539 
00540   SerialCom serial;
00541   ros::Timer timer1=serial.serial_node_handle_.createTimer(ros::Duration(0.025),&SerialCom::timerPackGet,&serial);
00542 
00543   ros::spin();
00544 
00545   return 0;
00546 }
00547 
00548 


dlut_move_base
Author(s): Zhuang Yan,Yan Fei,Dong Bingbing/zhuang@dlut.edu.cn
autogenerated on Sun Jan 5 2014 11:05:04