void buttonCallback (std_msgs::Bool msg)
{
if (msg.data)
changeRunMode (M_AUTO);
else
changeRunMode (M_IDLE);
}
void mrMainModeCallback (std_msgs::String msg)
{
if (msg.data == "auto")
changeRunMode (M_AUTO);
else if (msg.data == "idle")
changeRunMode (M_IDLE);
else
changeRunMode (M_MANUAL);
}
//-----------------------------------------------------------------------------
void MainWindow::keyPressEvent(QKeyEvent* e)
{
if ((e->key() == Qt::Key_W) || (e->key()==Qt::Key_Up))
m_frontDown=true;
else if ((e->key() == Qt::Key_S) || (e->key()==Qt::Key_Down))
m_backDown=true;
else if ((e->key() == Qt::Key_A) || (e->key()==Qt::Key_Left))
m_leftDown=true;
else if ((e->key() == Qt::Key_D) || (e->key()==Qt::Key_Right))
m_rightDown=true;
else if (e->key() == Qt::Key_R)
{
initStates();
}
else if (e->key() == Qt::Key_1)
resample();
else if (e->key() == Qt::Key_2)
drift();
else if (e->key() == Qt::Key_3)
diffuse();
else if (e->key() == Qt::Key_4)
measure();
else if (e->key() == Qt::Key_Tab)
changeRunMode();
else
e->ignore();
}
int main()
{
// Setup ROS Arguments
char** argv = NULL;
int argc = 0;
// Init ROS Node
ros::init (argc, argv, "MR_Button");
ros::NodeHandle nh;
ros::NodeHandle pNh ("~");
// Topic names
std::string mrMainRunSrv, buttonSub, obstacleDetectorSub, mrMainModeSub;
pNh.param<std::string> ("mr_button_sub", buttonSub, "/mrButton/status");
pNh.param<std::string> ("mr_obstacle_detector", obstacleDetectorSub, "/mrObstacleDetector/status");
pNh.param<std::string> ("mr_main_mode_sub", mrMainModeSub, "/mrMain/mode");
pNh.param<std::string> ("mr_main_run_srv", mrMainRunSrv, "/mrMain/run");
// Service
_srvMrMainRun = nh.serviceClient<mr_main::run>(mrMainRunSrv);
// Subscriber
ros::Subscriber subButton = nh.subscribe (buttonSub, 1, buttonCallback);
ros::Subscriber subObstacleDetector = nh.subscribe (obstacleDetectorSub, 1, obstacleDetectorCallback);
ros::Subscriber subMrMainMode = nh.subscribe (mrMainModeSub, 1, mrMainModeCallback);
// Get serial data parameters
int baudRate;
std::string port;
pNh.param<int> ("baud_rate", baudRate, 115200);
pNh.param<std::string> ("port", port, "/dev/serial/by-id/usb-Texas_Instruments_In-Circuit_Debug_Interface_0E203B83-if00");
// Open connection
_serialConnection = new serial::Serial (port.c_str(), baudRate, serial::Timeout::simpleTimeout (50));
// Check if connection is ok
if (!_serialConnection->isOpen())
{
ROS_ERROR ("Error opening connection!");
_serialConnection->close();
return 0;
}
else
ROS_INFO ("Successfully connected!");
// Start serial threads
boost::thread readThread(readSerialThread);
boost::thread writeThread(writeSerialThread);
// Sleep for a second
ros::Duration(2).sleep();
// Change mode to idle
changeRunMode (M_IDLE);
// ROS Spin: Handle callbacks
while (!ros::isShuttingDown())
ros::spin();
// Close connection
changeRunMode (M_OFF);
ros::Duration(2).sleep();
readThread.interrupt();
writeThread.interrupt();
_serialConnection->close();
// Return
return 0;
}
