int main(int argc, char **argv)
{
ros::init(argc, argv, "syncboard_node");
ros::NodeHandle nh;
Move mover;
// Advertise topics
ros::Publisher chatter_pub = nh.advertise<geometry_msgs::Twist>("/camera_stitch/cmd_vel", 1);
ros::Publisher timestamps_pub = nh.advertise<std_msgs::String>("syncbox_timestamps", 1);
//Synboard stuff
std::string port;
int baud;
nh.param<std::string>("port", port, "/dev/ttyUSB0");
nh.param("baud", baud, 9600);
syncboard.setTimeout(serial::Timeout::max(), 500, 0, 500, 0);
syncboard.setPort(port);
syncboard.setBaudrate(baud);
syncboard.open();
mover.handshake();
while(ros::ok()){
ros::spinOnce();
ros::Rate(10).sleep();
}
syncboard.write("s");
return 0;
}
bool open() {
bool device_opened = false;
if (is_open()) {
if (debug)
std::cout << "serial already opened" << std::endl;
} else {
m_serial.setPort(m_device_port);
m_serial.setBaudrate(115200);
//serial::Timeout t(1000, 1000);
m_serial.setTimeout(serial::Timeout::simpleTimeout(2000));
m_serial.open();
// Open port
if (m_serial.isOpen()) {
m_serial.flush();
// Check if the device is well a Wattsup
if (identify()) {
m_serial.flush();
device_opened = true;
} else {
m_serial.close();
}
}
}
return device_opened;
}
bool open(string portname, int baudrate){
port.setPort(portname);
port.setBaudrate(baudrate);
serial::Timeout t = serial::Timeout::simpleTimeout(1000);
port.setTimeout(t);
try{
port.open();
} catch (const serial::IOException& ex){
ROS_ERROR_THROTTLE(10, "IOException while attempting to open serial port '%s': %s", portname.c_str(), ex.what());
}
if(port.isOpen()){
port.setPort("kbot_bridge");
return true;
}
return false;
}