int main(int argc, char **argv)
{
/* ros messages */
msgs::serial s_rx_msg_;
msgs::serial s_tx_msg_;
/* parameters */
std::string device;
std::string publisher_topic;
std::string subscriber_topic;
int baudrate;
int term;
/* initialize ros usage */
ros::init(argc, argv, "vic_serial_interface");
ros::Publisher s_publisher;
ros::Subscriber s_subscriber;
/* private nodehandlers */
ros::NodeHandle nh;
ros::NodeHandle n("~");
/* read parameters from ros parameter server if available otherwise use default values */
n.param<std::string> ("device", device, "/dev/ttyS0");
n.param<std::string> ("publisher_topic", publisher_topic, "S0_rx_msg");
n.param<std::string> ("subscriber_topic", subscriber_topic, "S0_tx_msg");
n.param<int> ("termination_character", term,10);
n.param<int> ("baudrate", baudrate, 115200);
s_publisher = nh.advertise<msgs::serial> (publisher_topic.c_str(), 20,1);
serialInterface serialInterface(s_publisher);
serialInterface.term_char = (char)term;
/* keep trying until connection is made */
ros::Rate r(2);
while(serialInterface.openDevice(device, baudrate) && ros::ok())
{
r.sleep();
ros::spinOnce();
}
if(ros::ok())
{
s_subscriber = nh.subscribe<msgs::serial> (subscriber_topic.c_str(), 20, &serialInterface::writeHandler, &serialInterface);
ros::spin();
}
return 0;
}
int main(int argc, char **argv)
{
/* ros messages */
fmMsgs::serial_bin s_rx_msg_;
fmMsgs::serial_bin s_tx_msg_;
/* parameters */
std::string device;
std::string publisher_topic;
std::string subscriber_topic;
int baudrate;
/* initialize ros usage */
ros::init(argc, argv, "serial_binary_interface");
ros::Publisher s_publisher;
ros::Subscriber s_subscriber;
/* private nodehandlers */
ros::NodeHandle nh;
ros::NodeHandle n("~");
/* read parameters from ros parameter server if available otherwise use default values */
n.param<std::string> ("device", device, "/dev/ttyUSB0");
n.param<std::string> ("publisher_topic", publisher_topic, "S0_rx_msg");
n.param<std::string> ("subscriber_topic", subscriber_topic, "S0_tx_msg");
n.param<int> ("baudrate", baudrate, 38400);
s_publisher = nh.advertise<fmMsgs::serial_bin> (publisher_topic.c_str(), 1);
serialInterface serialInterface(s_publisher);
serialInterface.openDevice(device, baudrate);
s_subscriber = nh.subscribe<fmMsgs::serial_bin> (subscriber_topic.c_str(), 1000, &serialInterface::writeHandler, &serialInterface);
ros::spin();
return 0;
}
void BTConnection::disconnectDevice( const QString& aBTAddress, const QString& aDevice )
{
FUNCTION_CALL_TRACE;
QDBusInterface managerInterface( BLUEZ_DEST, "/", BLUEZ_MANAGER_INTERFACE, QDBusConnection::systemBus() );
if( !managerInterface.isValid() ) {
LOG_CRITICAL( "Could not find BlueZ manager interface" );
return;
}
QDBusReply<QDBusObjectPath> pathReply = managerInterface.call( QLatin1String( GET_DEFAULT_ADAPTER ) );
if( !pathReply.isValid() ) {
LOG_CRITICAL( "Could not find default adapter path" );
LOG_CRITICAL( "Reason:" << pathReply.error() );
return;
}
QString defaultAdapterPath = pathReply.value().path();
LOG_DEBUG("Using adapter path: " << defaultAdapterPath );
QDBusInterface adapterInterface( BLUEZ_DEST, defaultAdapterPath, BLUEZ_ADAPTER_INTERFACE, QDBusConnection::systemBus() );
if( !adapterInterface.isValid() ) {
LOG_CRITICAL( "Could not find adapter interface: " << adapterInterface.lastError() );
return;
}
pathReply = adapterInterface.call( QLatin1String( FIND_DEVICE ), aBTAddress );
if( !pathReply.isValid() ) {
LOG_CRITICAL( "Couldn't find device " << aBTAddress );
LOG_CRITICAL( "Reason:" << pathReply.error() );
return;
}
QString devicePath = pathReply.value().path();
LOG_DEBUG( "Using path" << devicePath << "for device " << aBTAddress );
QDBusInterface serialInterface( BLUEZ_DEST, devicePath, BLUEZ_SERIAL_INTERFACE, QDBusConnection::systemBus() );
if( !serialInterface.isValid() ) {
LOG_CRITICAL( "Could not find serial interface: " << serialInterface.lastError() );
return;
}
QDBusReply<void> voidReply = serialInterface.call( QLatin1String( DISCONNECT ), aDevice );
if( !voidReply.isValid() ) {
LOG_CRITICAL( "Device disconnection failed" );
LOG_CRITICAL( "Reason:" << voidReply.error() );
return;
}
LOG_DEBUG( "Device disconnected:" << aBTAddress );
voidReply = adapterInterface.call( RELEASE_SESSION );
if( !voidReply.isValid() ) {
LOG_CRITICAL( "Session release failed" );
LOG_CRITICAL( "Reason:" << voidReply.error() );
return;
}
LOG_DEBUG( "BT session closed" );
iDevice.clear();
}
QString BTConnection::connectDevice( const QString& aBTAddress, const QString& aServiceUUID )
{
FUNCTION_CALL_TRACE;
QDBusInterface managerInterface( BLUEZ_DEST, "/", BLUEZ_MANAGER_INTERFACE, QDBusConnection::systemBus() );
if( !managerInterface.isValid() ) {
LOG_CRITICAL( "Could not find BlueZ manager interface" );
return "";
}
QDBusReply<QDBusObjectPath> pathReply = managerInterface.call( QLatin1String( GET_DEFAULT_ADAPTER ) );
if( !pathReply.isValid() ) {
LOG_CRITICAL( "Could not find default adapter path:" << pathReply.error() );
return "";
}
QString defaultAdapterPath = pathReply.value().path();
LOG_DEBUG("Using adapter path: " << defaultAdapterPath );
QDBusInterface adapterInterface( BLUEZ_DEST, defaultAdapterPath, BLUEZ_ADAPTER_INTERFACE, QDBusConnection::systemBus() );
if( !adapterInterface.isValid() ) {
LOG_CRITICAL( "Could not find adapter interface: " << adapterInterface.lastError() );
return "";
}
QDBusReply<void> voidReply = adapterInterface.call( QLatin1String( REQUEST_SESSION ) );
if( !voidReply.isValid() ) {
LOG_CRITICAL( "Session request failed" );
LOG_CRITICAL( "Reason:" << voidReply.error() );
}
LOG_DEBUG( "BT session created" );
pathReply = adapterInterface.call( QLatin1String( FIND_DEVICE ), aBTAddress );
if( !pathReply.isValid() ) {
LOG_WARNING( "Couldn't find device " << aBTAddress << "Reason:" << pathReply.error() );
LOG_DEBUG( "Create Device :" << aBTAddress );
pathReply = adapterInterface.call( QLatin1String( CREATE_DEVICE ), aBTAddress );
if (pathReply.isValid()){
LOG_DEBUG( "Create Paired Device :" << aBTAddress << "Path :" << pathReply.value().path() );
QDBusReply<QDBusObjectPath> reply =
adapterInterface.call(QLatin1String( CREATE_PAIRED_DEVICE ),
aBTAddress, qVariantFromValue(pathReply.value()), QString());
if( !reply.isValid() ) {
LOG_CRITICAL( "Pairing failed Reason:" << reply.error());
}
}
}
if( !pathReply.isValid() ) {
LOG_CRITICAL( "Couldn't find device " << aBTAddress );
LOG_CRITICAL( "Reason:" << pathReply.error() );
adapterInterface.call( QLatin1String( RELEASE_SESSION ) );
LOG_CRITICAL( "BT session closed" );
return "";
}
QString devicePath = pathReply.value().path();
LOG_DEBUG( "Using path" << devicePath << "for device " << aBTAddress );
QDBusInterface serialInterface( BLUEZ_DEST, devicePath, BLUEZ_SERIAL_INTERFACE, QDBusConnection::systemBus() );
if( !serialInterface.isValid() ) {
LOG_CRITICAL( "Could not find serial interface: " << serialInterface.lastError() );
adapterInterface.call( QLatin1String( RELEASE_SESSION ) );
LOG_CRITICAL( "BT session closed" );
return "";
}
QDBusReply<QString> stringReply = serialInterface.call( QLatin1String( CONNECT ), aServiceUUID );
if( !stringReply.isValid() ) {
LOG_CRITICAL( "Could not connect to device " << devicePath << " with service uuid " << aServiceUUID );
LOG_CRITICAL( "Reason:" << stringReply.error() );
adapterInterface.call( QLatin1String( RELEASE_SESSION ) );
LOG_CRITICAL( "BT session closed" );
return "";
}
LOG_DEBUG("Device connected:" << aBTAddress );
return stringReply.value();
}
