Ticket #5339: test_serial.cpp

#include "ros/ros.h"

#include <string>
#include <iostream>

#include "serial.h"

Serial *serial;

std::string toHex(std::string input) {
    std::stringstream ss;
    for(unsigned int i = 0; i != input.length(); i++) {
        char temp[4];
        sprintf(temp, "%.2X", input[i]);
        ss << " ";
        if(input[i] == 0x0A)
            ss << "LF";
        else if(input[i] == 0x0D)
            ss << "NL";
        else
            ss << input[i];
        ss << " " << std::hex << temp;
    }
    return ss.str();
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "serial_test_node");

    ros::NodeHandle n;

    std::string port("/dev/tty.usbserial-A900cfJA");
    // std::string port("/dev/tty.usbmodemfa141");

    serial = new Serial(port, 9600, 250);

    ros::Rate loop_rate(0.5);

    int count = 0;
    while (ros::ok() and count != 30) {
        // serial->write("Testing.");
        // ROS_INFO("Out of write");
        std::string result = serial->read(1);
        std::cout << ">" << result << std::endl;


        // ROS_INFO("Here.");

        // ROS_INFO(result.c_str());
        // ROS_INFO("%d,%s", result.length(), toHex(result).c_str());


        ros::spinOnce();

        // loop_rate.sleep();
        // count += 1;
    }

    return 0;
}