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