motor_serial.cc
Go to the documentation of this file.
00001 
00031 #include <ubiquity_motor/motor_serial.h>
00032 #include <ros/ros.h>
00033 #include <serial/serial.h>
00034 
00035 MotorSerial::MotorSerial(const std::string &port, uint32_t baud_rate, double loopRate) {
00036     have_input = false;
00037 
00038     // Make sure baud rate is valid
00039     switch (baud_rate) {
00040         case 110 :
00041         case 300 :
00042         case 600 :
00043         case 1200 :
00044         case 2400 :
00045         case 4800 :
00046         case 9600 :
00047         case 14400 :
00048         case 19200 :
00049         case 28800 :
00050         case 38400 :
00051         case 56000 :
00052         case 57600 :
00053         case 115200 :
00054         case 128000 :
00055         case 153600 :
00056         case 230400 :
00057         case 256000 :
00058         case 460800 :
00059         case 921600 :
00060             this->_baud_rate = baud_rate;
00061             break;
00062         default :
00063             this->_baud_rate = 9600;
00064             break;
00065     }
00066 
00067     // TODO verify that port is valid
00068     this->_port = port;
00069 
00070     motors = new serial::Serial(_port, _baud_rate, serial::Timeout::simpleTimeout(10000));
00071 
00072     serial_loop_rate = new ros::Rate(loopRate);
00073 
00074     serial_thread = new boost::thread(&MotorSerial::SerialThread, this);
00075 }
00076 
00077 MotorSerial::~MotorSerial() {
00078     serial_thread->interrupt();
00079     serial_thread->join();
00080     motors->close();
00081     delete motors;
00082     delete serial_thread;
00083     delete serial_loop_rate;
00084 }
00085 
00086 int MotorSerial::transmitCommand(MotorMessage command) {
00087     // Make sure to lock mutex before accessing the input fifo
00088     input_mtx_.lock();
00089     this->input.push(command); // add latest command to end of fifo
00090     this->have_input = true; //Used to avoid input locking
00091     input_mtx_.unlock();
00092     return 0;
00093 }
00094 
00095 int MotorSerial::transmitCommands(std::vector<MotorMessage> commands) {
00096     // Make sure to lock mutex before accessing the input fifo
00097     input_mtx_.lock();
00098     for (std::vector<MotorMessage>::iterator it = commands.begin(); it != commands.end(); ++it)
00099     {
00100         this->input.push(*it);
00101         this->have_input = true; //Used to avoid input locking
00102     }
00103     input_mtx_.unlock();
00104     return 0;
00105 }
00106 
00107 MotorMessage MotorSerial::receiveCommand() {
00108     MotorMessage mc;
00109     
00110     output_mtx_.lock();
00111     if (!this->output.empty()) {
00112         mc = this->output.front();
00113         this->output.pop();
00114     }
00115     output_mtx_.unlock();
00116     return mc;
00117 }
00118 
00119 int MotorSerial::commandAvailable() {
00120     // If have_output is false return false
00121     // if it is true, then verify there is output and return true
00122     // if verification fails make sure have_output is false
00123     if(!this->have_output) {
00124         return false;
00125     }
00126 
00127     output_mtx_.lock();
00128     int out = !(this->output.empty());
00129     if(!out) {
00130         this->have_output = false;
00131     }
00132     output_mtx_.unlock();
00133 
00134     return out;
00135 }
00136 
00137 int MotorSerial::inputAvailable() {
00138     // If have_input is false return false
00139     // if it is true, then verify there is input and return true
00140     // if verification fails make sure have_input is false
00141 
00142     if (!this->have_input) {
00143         return false;
00144     }
00145 
00146     input_mtx_.lock();
00147     int out = !(this->input.empty());
00148     if (!out) {
00149         this->have_input = false;
00150     }
00151     input_mtx_.unlock();
00152 
00153     return out;
00154 }
00155 
00156 MotorMessage MotorSerial::getInputCommand() {
00157     MotorMessage mc;
00158     input_mtx_.lock();
00159     if (!this->input.empty()) {
00160         mc = this->input.front();
00161         this->input.pop();
00162     }
00163     input_mtx_.unlock();
00164     return mc;
00165 }
00166 
00167 void MotorSerial::appendOutput(MotorMessage command) {
00168     output_mtx_.lock();
00169     this->output.push(command);
00170     this->have_output = true;
00171     output_mtx_.unlock();
00172 }
00173 
00174 void MotorSerial::SerialThread() {
00175     try {
00176         std::vector<uint8_t> in(0);
00177         bool failed_update = false;
00178 
00179         while (motors->isOpen()) {
00180 
00181             while (motors->available() >= (failed_update ? 1 : 9)) {
00182                 std::vector<uint8_t> innew(0);
00183                 motors->read(innew, failed_update ? 1 : 9);
00184                 in.insert(in.end(), innew.begin(), innew.end());
00185 
00186                 while (in.size() > 9) {
00187                     in.erase(in.begin());
00188                 }
00189 
00190                 //ROS_ERROR("Len:%d - fail %d  innew %d", (int) in.size(), failed_update, innew.size());
00191                 MotorMessage mc;
00192                 int error_code = mc.deserialize(in);
00193 
00194                 if (error_code == 0) {
00195                     //ROS_ERROR("appendOutput");
00196                     appendOutput(mc);
00197                     failed_update = false;
00198                 } else if (error_code == 1) {
00199                     failed_update = true;
00200                     //ROS_ERROR("Invalid delim");
00201                     char str[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
00202                     for (int i = 0; i < in.size() && i < 9; i++) {
00203                         str[i] = in.at(i);
00204                     }
00205                     ROS_ERROR("REJECT: %s", str);
00206                 } else {
00207                     failed_update = true;
00208                     ROS_ERROR("DESERIALIZATION ERROR! - %d", error_code);
00209                 }
00210             }
00211 
00212             bool did_update = false;
00213             while (inputAvailable()) {
00214                 did_update = true;
00215 
00216                 std::vector<uint8_t> out(9);
00217 
00218                 out = getInputCommand().serialize();
00219                 // ROS_ERROR("out %x %x %x %x %x %x %x %x %x",
00220                 //      out[0],
00221                 //      out[1],
00222                 //      out[2],
00223                 //      out[3],
00224                 //      out[4],
00225                 //      out[5],
00226                 //      out[6],
00227                 //      out[7],
00228                 //      out[8]);
00229                 motors->write(out);
00230             }
00231 
00232             if (did_update) {
00233                 motors->flushOutput();
00234             }
00235 
00236             // boost::posix_time::milliseconds loopDelay(10);
00237             // boost::this_thread::sleep(loopDelay);
00238             boost::this_thread::interruption_point();
00239             serial_loop_rate->sleep();
00240         }
00241 
00242     }
00243     catch (const boost::thread_interrupted &e) {
00244         // ROS_INFO("boost::thread_interrupted");
00245         motors->close();
00246     }
00247     catch (const serial::IOException &e) {
00248         ROS_ERROR("%s", e.what());
00249     }
00250     catch (const serial::PortNotOpenedException &e) {
00251         ROS_ERROR("%s", e.what());
00252     }
00253     catch (...) {
00254         ROS_ERROR("Unknown Error");
00255         throw;
00256     }
00257 }


ubiquity_motor
Author(s):
autogenerated on Thu Jun 6 2019 18:33:27