/** * 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; }