Example #1
0
/**
 * The start the Arduino node
 *
 * This read command line arguments if applicable the opens the serial port.
 * Then reads the serial port at 400 Hz.
 *
 */
int main(int argc, char **argv) {

    std::string usb_port_name = ARDUINO_SERIAL_PORT_DEFAULT;
    int baudrate = ARDUINO_BAUDRATE_DEFAULT;
    int opt;

    //to initialize ros with command line arguments and name of the node
    ros::init(argc, argv, "arduino_handler");

    ros::NodeHandle n;

    //get any params/ set any params
    n.param<std::string>("dev", usb_port_name, usb_port_name);
    n.param<int>("baud_rate", baudrate, baudrate);

    //commandline arguments take precedance to params
    while(( opt = getopt(argc, argv, "u:b:")) != -1) {
        switch (opt) {
        case 'u':
            usb_port_name.assign(optarg);
            n.setParam("dev",usb_port_name);
            break;
        case 'b':
            baudrate = atoi(optarg);
            n.setParam("baud_rate", baudrate);
            break;
        default:
            break;
        }
    }

    bIsConnected = false;
    nmeaChecksumFailedCount = 0;

    //advertise(topic_name, queue_size, latch)
    tIsAduinoConnected = n.advertise<std_msgs::Bool>(IsArduinoConnected_T, 1, true);
    std_msgs::Bool msg_isArduinoConnected;
    msg_isArduinoConnected.data = bIsConnected;
    tIsAduinoConnected.publish(msg_isArduinoConnected);

    tIMUData = n.advertise<ghost::imu>(IMU_T, 1, true);

    Serial serial;

    if (serial.open_serial_port(usb_port_name, baudrate) == false) {
        ros::shutdown();
    }

    ros::Timer watchdogTimer = n.createTimer(ros::Duration(ARDUINO_WATCHDOG_DURATION), arduinoWatchdog);
    ros::Timer heartbeatTimer = n.createTimer(ros::Duration(ARDUINO_HEARTBEAT_DURATION), arduinoHeartBeat);

    //specify the speed of the loop
    ros::Rate loop(ARDUINO_LOOP_RATE);

    std::string inBuf;

    //loop until program is closed (Ctrl-C or ros::shutdown())
    while (ros::ok()) {

        //lastConnectedTime.sec

        inBuf += serial.read_serial_data();
        parse_nmea_message(inBuf);

        //to check if subscribed topics have updated
        ros::spinOnce();

        //sleep until next loop time
        loop.sleep();
    }

    watchdogTimer.stop();
    heartbeatTimer.start();

    serial.close_serial_port();

    return 0;
}