motor_hokuyo.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 <stdio.h>
00033 #include <string.h>
00034 #include <sys/types.h>
00035 #include <errno.h>
00036 #include <sys/stat.h>
00037 #include <fcntl.h>
00038 #include <unistd.h>
00039 #include <termios.h>
00040 #include <stdlib.h>
00041 #include "time.h" 
00042 #include <math.h>
00043 
00044 #include <ros/ros.h> 
00045 #include <std_msgs/Bool.h>
00046 #include <std_msgs/Int32.h>
00047 #include <dlut_motor_hokuyo/motor.h>
00048 
00049 #define DEFAULT_PORT 1
00050 #define DEFAULT_BAUDRATE 19200
00051 #define DEFAULT_DIRECTION 0 //counterclockwise
00052 #define DEFAULT_FRE 1000
00053 #define DEFAULT_ANGEL 720
00054 
00055 using namespace std;
00056 class SickMotor
00057 {
00058 public:
00059   SickMotor();
00060   ~SickMotor();
00061   int openPort(int fd_, int comport);//open the serial port
00062   int setOpt(int fd_,int nspeed,int nbits,char nevent,int nstop);//config the serial port
00063   int getPulseNum(int _degree);//calculate the number of the pulse according to the rotate degree of the motor
00064   //void getDirection(char* _dir,char &motor_dir_);
00065   void delayms(long int i);//the delay function,the unit is ms
00066   int comInit();
00067 
00068   void sickMotorCallBack(const std_msgs::Bool &is_run);
00069   ros::NodeHandle sick_motor_nh_;
00070   char motor_dir_;//indicate the rotating direction of the sick
00071 
00072   std_msgs::Bool is_motor_run_;
00073 
00074   ros::Subscriber is_motor_run_sub_;
00075   ros::Publisher motor_pub_;
00076 private:
00077   int port_,baudrate_;
00078   int fd_;
00079   int nwrite_; 
00080 
00081   char command_buff_[30];       
00082 };
00083 
00084 SickMotor::SickMotor()
00085 {
00086   sick_motor_nh_.param("Port",port_,DEFAULT_PORT);//get the value of port_ from parameter server
00087   sick_motor_nh_.param("BaudRate",baudrate_,DEFAULT_BAUDRATE);
00088   comInit();
00089   is_motor_run_sub_ = sick_motor_nh_.subscribe("is_run", 1, &SickMotor::sickMotorCallBack,this);
00090   motor_pub_ = sick_motor_nh_.advertise<dlut_motor_hokuyo::motor>("motor_param",1000);
00091   motor_dir_ = '0';//counterclockwise rotating direction
00092   is_motor_run_.data = false;
00093 }
00094 
00095 SickMotor::~SickMotor()
00096 {
00097     close(fd_);
00098 }
00099 
00100 //open the serial port
00101 int SickMotor::openPort(int fd_, int comport)
00102 {
00103   if(comport == 1)//port 1
00104   {
00105     fd_ = open("/dev/ttyS0",O_RDWR|O_NOCTTY|O_NDELAY);
00106     
00107     if(-1  ==  fd_)
00108     {
00109       ROS_INFO("failed to open port 1!");
00110       return(-1);
00111     }
00112   }
00113   else if(comport == 2)//port 2
00114   {
00115     fd_ = open("/dev/ttyS1",O_RDWR|O_NOCTTY|O_NDELAY);
00116     if(-1 == fd_)
00117     {
00118       ROS_INFO("failed to open port 2!");
00119       return(-1);
00120     }
00121   }
00122   else if(comport == 3)//port 3
00123   {
00124     fd_ = open("/dev/ttyS2",O_RDWR|O_NOCTTY|O_NDELAY);
00125         
00126     if(-1 == fd_)
00127     {
00128       ROS_INFO("failed to open port 3!");
00129       return(-1);
00130     }
00131   }
00132 
00133   if(fcntl(fd_,F_SETFL,0) < 0)
00134     ROS_INFO("fcntl failed!");
00135   else
00136     ROS_INFO("fcntl=%d.",fcntl(fd_,F_SETFL,0));
00137 
00138   if(isatty(STDIN_FILENO) == 0)
00139     ROS_INFO("standsrd input is not a terminal device.");
00140   else
00141     ROS_INFO("is a tty sucess.");
00142 
00143   ROS_INFO("the return value of the serial open function=%d,!=-1,indicates succeed to open the serial.",fd_);
00144         
00145   return fd_;
00146 }
00147 
00148 //setup the serial port
00149 int SickMotor::setOpt(int fd_,int nspeed,int nbits,char nevent,int nstop)
00150 {
00151   struct termios newtio,oldtio;
00152 
00153   //check the parameter of the serial port to see whether there is error or not
00154   if(tcgetattr(fd_,&oldtio) != 0)
00155   {
00156     ROS_INFO("failed to setup the serial,failed to save the serial value!");
00157     return -1;  
00158   }
00159 
00160   bzero(&newtio,sizeof(newtio));
00161 
00162   newtio.c_cflag |= CLOCAL | CREAD;
00163   newtio.c_cflag &= ~CSIZE;
00164 
00165   switch(nbits)
00166   {
00167     case 7:
00168       newtio.c_cflag |= CS7;
00169       break;
00170     case 8:
00171       newtio.c_cflag |= CS8;
00172       break;    
00173   }
00174 
00175   switch(nevent)
00176   {
00177     case 'O': 
00178       newtio.c_cflag |= PARENB;
00179       newtio.c_cflag |= PARODD;
00180       newtio.c_iflag |= (INPCK | ISTRIP);
00181       break;
00182     case 'E':
00183       newtio.c_iflag |= (INPCK | ISTRIP);
00184       newtio.c_cflag |= PARENB ;
00185       newtio.c_cflag &= ~ PARODD;
00186       break;
00187     case 'N':
00188       newtio.c_cflag &= ~ PARENB;
00189       break;            
00190   }
00191 
00192   //setup the baud rate
00193   switch(nspeed)
00194   {
00195     case 2400:
00196       cfsetispeed(&newtio,B2400);
00197       cfsetospeed(&newtio,B2400);
00198       break;            
00199     case 4800:
00200       cfsetispeed(&newtio,B4800);
00201       cfsetospeed(&newtio,B4800);
00202       break;    
00203     case 9600:
00204       cfsetispeed(&newtio,B9600);
00205       cfsetospeed(&newtio,B9600);
00206       break;    
00207     case 19200:
00208       cfsetispeed(&newtio,B19200);
00209       cfsetospeed(&newtio,B19200);
00210       break;    
00211     case 38400:
00212       cfsetispeed(&newtio,B38400);
00213       cfsetospeed(&newtio,B38400);
00214       break;
00215     case 57600:
00216       cfsetispeed(&newtio,B57600);
00217       cfsetospeed(&newtio,B57600);
00218       break;            
00219     case 115200:
00220       cfsetispeed(&newtio,B115200);
00221       cfsetospeed(&newtio,B115200);
00222       break;    
00223     case 460800:
00224       cfsetispeed(&newtio,B460800);
00225       cfsetospeed(&newtio,B460800);
00226       break;    
00227     default:
00228       cfsetispeed(&newtio,B9600);
00229       cfsetospeed(&newtio,B9600);
00230       break;            
00231   }
00232 
00233   //setup the stop bit
00234   if(nstop == 1)
00235     newtio.c_cflag &= ~ CSTOPB;
00236   else if(nstop == 2)
00237     newtio.c_cflag |= CSTOPB;
00238         
00239   //setup the waitting time and the minimum amount of the characters received 
00240   newtio.c_cc[VTIME] = 0;
00241   newtio.c_cc[VMIN] = 0;
00242 
00243   //deal with the characters which were not received.
00244   tcflush(fd_,TCIFLUSH);
00245 
00246   //activate the new serial setup
00247   if((tcsetattr(fd_,TCSANOW,&newtio)) != 0)
00248   {
00249     ROS_INFO("failed to activate the new serial setup!");
00250     return -1;
00251   }
00252         
00253   ROS_INFO("serial setup success!\n");
00254 
00255   return 0;
00256 }
00257 
00258 int SickMotor::getPulseNum(int _degree)
00259 {
00260    return (_degree*8240/360);
00261 
00262 }
00263 /*
00264 void SickMotor::getDirection(char* _dir,char &motor_dir_)
00265 {
00266   if(strcmp(_dir,"0") == 0)
00267   {
00268     motor_dir_ = '0';
00269   }
00270   
00271   if(strcmp(_dir,"1") == 0)
00272   {
00273     motor_dir_ = '1';
00274   }
00275 }
00276 */
00277 void SickMotor::delayms(long int i)
00278 {
00279   usleep(1000*i);
00280 }
00281 
00282 int SickMotor::comInit()
00283 {
00284   int i;
00285 
00286   if((fd_ = openPort(fd_,port_)) < 0)
00287   {
00288     ROS_INFO("failed to setup the serial!");
00289     return 0;
00290   }
00291 
00292   if((i = setOpt(fd_,baudrate_,8,'N',1)) < 0)
00293   {
00294     ROS_INFO("failed to setup the serial!");
00295     return 0;
00296   }
00297 
00298   ROS_INFO("the serial openned,setup the serial successed。the file operator = %d\n",fd_); 
00299   
00300   return 0;
00301 }
00302 
00303 void SickMotor::sickMotorCallBack(const std_msgs::Bool &is_run)
00304 {
00305   is_motor_run_.data = is_run.data;
00306   if(is_motor_run_.data == true)
00307   {
00308     int motor_pulseNum = getPulseNum(DEFAULT_ANGEL); 
00309     sprintf(command_buff_,"M%c%06d%06d",motor_dir_,DEFAULT_FRE,motor_pulseNum);//generate the control command
00310     ROS_INFO("%s\n",command_buff_);
00311     nwrite_ = write(fd_,command_buff_,strlen(command_buff_));//send the control command to the serial port
00312     if(nwrite_ < 0)
00313     {
00314       ROS_INFO("serial send data error!\n");  
00315     }
00316   }
00317 }
00318 
00319 int main(int argc,char **argv)
00320 {
00321   ros::init(argc, argv, "SickMotor");   
00322   SickMotor sm;
00323 
00324   sleep(1);  
00325   dlut_motor_hokuyo::motor motor_param;
00326   motor_param.motor_freq.data = DEFAULT_FRE;
00327   motor_param.num_of_pulse_per_circle.data = 8240;
00328   sm.motor_pub_.publish(motor_param);
00329 
00330   ros::spin();  
00331   
00332   return 0;
00333 }


dlut_motor_hokuyo
Author(s): Zhuang Yan , Yan Fei, Dong Bing Bing
autogenerated on Sun Oct 5 2014 23:29:35