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 Move::handshake(){
int counter = 0;
while(ros::ok()){
syncboard.flush();
syncboard.write("v");
std::string result = syncboard.readline();
if (result.length() > 0){
ROS_INFO("Connected to syncboard.");
return true;
}
if(counter++ > 50){
ROS_WARN_ONCE("Connecting to syncboard is taking longer than expected.");
}
ros::Rate(10).sleep();
}
ROS_WARN("Syncboard handshake failed.");
return false;
}
void close() {
if (m_closing) return;
m_closing = true;
if (is_open()) {
// Internal logging 600s
Packet p;
WattsupCommand::SetupInternalLogging600s(p);
write(p);
m_serial.flush();
}
m_serial.close();
m_buf_len = 0;
m_commands.clear();
m_closing = false;
//this->fileLog("Device closed ! [" + m_device_port + "]");
}