motor_serial.cc

Go to the documentation of this file.

#include <ros/ros.h>
#include <serial/serial.h>
#include <ubiquity_motor/motor_serial.h>

MotorSerial::MotorSerial(const std::string& port, uint32_t baud_rate)
    : motors(port, baud_rate, serial::Timeout::simpleTimeout(100)), 
      serial_errors(0), error_threshold(20) {
    serial_thread = boost::thread(&MotorSerial::SerialThread, this);
}

MotorSerial::~MotorSerial() {
    serial_thread.interrupt();
    serial_thread.join();
    motors.close();
}

int MotorSerial::transmitCommand(MotorMessage command) {
    RawMotorMessage out = command.serialize();
    ROS_DEBUG("out %02x %02x %02x %02x %02x %02x %02x %02x", out[0], out[1],
              out[2], out[3], out[4], out[5], out[6], out[7]);
    motors.write(out.c_array(), out.size());
    return 0;
}

int MotorSerial::transmitCommands(const std::vector<MotorMessage>& commands) {
    for (auto& command : commands) {
        RawMotorMessage out = command.serialize();
        ROS_DEBUG("out %02x %02x %02x %02x %02x %02x %02x %02x", out[0], out[1],
                  out[2], out[3], out[4], out[5], out[6], out[7]);
        motors.write(out.c_array(), out.size());
        boost::this_thread::sleep(boost::posix_time::milliseconds(2));
    }
    return 0;
}

MotorMessage MotorSerial::receiveCommand() {
    MotorMessage mc;
    if (!this->output.empty()) {
        mc = output.front_pop();
    }
    return mc;
}

int MotorSerial::commandAvailable() { return !output.fast_empty(); }

void MotorSerial::appendOutput(MotorMessage command) { output.push(command); }

void MotorSerial::SerialThread() {
    try {
        while (motors.isOpen()) {
            boost::this_thread::interruption_point();
            if (motors.waitReadable()) {
                RawMotorMessage innew = {0, 0, 0, 0, 0, 0, 0, 0};

                motors.read(innew.c_array(), 1);
                if (innew[0] != MotorMessage::delimeter) {
                    // The first byte was not the delimiter, so re-loop
                    if (++serial_errors > error_threshold) {
                        ROS_WARN("REJECT %02x", innew[0]);
                    }
                    continue;
                }

                // This will wait for the transmission time of 8 bytes
                motors.waitByteTimes(innew.size());

                // Read in next 7 bytes
                motors.read(&innew.c_array()[1], 7);
                ROS_DEBUG("Got message %x %x %x %x %x %x %x %x", innew[0],
                          innew[1], innew[2], innew[3], innew[4], innew[5],
                          innew[6], innew[7]);

                MotorMessage mc;
                int error_code = mc.deserialize(innew);
                if (error_code == 0) {
                    appendOutput(mc);
                    if (mc.getType() == MotorMessage::TYPE_ERROR) {
                        ROS_ERROR("GOT error from Firm 0x%02x",
                                  mc.getRegister());
                    }
                } else {
                    if (++serial_errors > error_threshold) {
                        if (error_code == MotorMessage::ERR_UNKNOWN_REGISTER) {
                            ROS_WARN_ONCE("Message deserialize found an unrecognized firmware register");
                        } else {
                            ROS_ERROR("DESERIALIZATION ERROR! - %d", error_code);
                        }
                    }
                }
            }
        }

    } catch (const boost::thread_interrupted& e) {
        motors.close();
    } catch (const serial::IOException& e) {
        ROS_ERROR("%s", e.what());
    } catch (const serial::PortNotOpenedException& e) {
        ROS_ERROR("%s", e.what());
    } catch (...) {
        ROS_ERROR("Unknown Error");
        throw;
    }
}