From 8ac4803ddce88a57f7ea2d0240f13c42c68b9d81 Mon Sep 17 00:00:00 2001 From: JGMonroy Date: Tue, 1 Jul 2014 12:40:06 +0200 Subject: [PATCH] Added SpeechSynth module. Moved module MQTT from Giraff_pkg(obsolete) to utils --- CMakeLists.txt | 4 +- MQTTMosquitto/CMQTTMosquitto.cpp | 780 +++++++++++++++++++++++++++++++ MQTTMosquitto/CMQTTMosquitto.h | 92 ++++ MQTTMosquitto/CMakeLists.txt | 26 ++ MQTTMosquitto/Constants.cpp | 9 + MQTTMosquitto/Constants.h | 13 + MQTTMosquitto/main.cpp | 51 ++ SpeechSynth/CMakeLists.txt | 16 + SpeechSynth/CSpeechSynth.cpp | 138 ++++++ SpeechSynth/CSpeechSynth.hpp | 58 +++ SpeechSynth/main.cpp | 46 ++ 11 files changed, 1232 insertions(+), 1 deletion(-) create mode 100644 MQTTMosquitto/CMQTTMosquitto.cpp create mode 100644 MQTTMosquitto/CMQTTMosquitto.h create mode 100644 MQTTMosquitto/CMakeLists.txt create mode 100644 MQTTMosquitto/Constants.cpp create mode 100644 MQTTMosquitto/Constants.h create mode 100644 MQTTMosquitto/main.cpp create mode 100644 SpeechSynth/CMakeLists.txt create mode 100644 SpeechSynth/CSpeechSynth.cpp create mode 100644 SpeechSynth/CSpeechSynth.hpp create mode 100644 SpeechSynth/main.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 3305795..fd50cb0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -32,4 +32,6 @@ DEFINE_MORA_PROGRAM(TextInterpreter) DEFINE_MORA_PROGRAM(SkypeComm) DEFINE_MORA_PROGRAM(TwitterAgent) DEFINE_MORA_PROGRAM(Wifi_Monitor) -DEFINE_MORA_PROGRAM(Wifi_KeepCoverage) \ No newline at end of file +DEFINE_MORA_PROGRAM(Wifi_KeepCoverage) +DEFINE_MORA_PROGRAM(MQTTMosquitto) +DEFINE_MORA_PROGRAM(SpeechSynth) \ No newline at end of file diff --git a/MQTTMosquitto/CMQTTMosquitto.cpp b/MQTTMosquitto/CMQTTMosquitto.cpp new file mode 100644 index 0000000..e37f8bd --- /dev/null +++ b/MQTTMosquitto/CMQTTMosquitto.cpp @@ -0,0 +1,780 @@ +/* +---------------------------------------------------------------------------+ + | Open MORA (MObile Robot Arquitecture) | + | | + | http://babel.isa.uma.es/mora/ | + | | + | Copyright (C) 2010 University of Malaga | + | | + | This software was written by the Machine Perception and Intelligent | + | Robotics (MAPIR) Lab, University of Malaga (Spain). | + | Contact: Jose-Luis Blanco | + | | + | This file is part of the MORA project. | + | | + | MORA is free software: you can redistribute it and/or modify | + | it under the terms of the GNU General Public License as published by | + | the Free Software Foundation, either version 3 of the License, or | + | (at your option) any later version. | + | | + | MORA is distributed in the hope that it will be useful, | + | but WITHOUT ANY WARRANTY; without even the implied warranty of | + | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | + | GNU General Public License for more details. | + | | + | You should have received a copy of the GNU General Public License | + | along with MORA. If not, see . | + | | + +---------------------------------------------------------------------------+ */ + + +/** @moos_module Module to enable the communication between OpenMORA and a MQTT client. + * MQTT is a machine-to-machine (M2M)/"Internet of Things" connectivity protocol, designed as an extremely lightweight publish/subscribe messaging transport. + * This module is used to communicate the Giraff robot (running the OpenMORA architecture), with an external MQTT client in the user side (Pilot, WebRTC, etc.). + */ + + +/* + MQTT Topic <-> OpenMORA variable + -------------------------------------------------------------- + + Giraff_Name/ClientACK -->PILOT_MQTT_ACK + + Giraff_Name/TopologyCommand + -GetTopology --> (Send Variable GRAPH in Topic Topology) + -ADD_NODE label type x y --> ADD_NODE + -ADD_ARC labelA labelB ArcType --> ADD ARC + -REMOVE_NODE label --> REMOVE_NODE + -REMOVE_ARCC labelA labelB ArcType --> REMOVE_AR + -MOVE_NODE label x y --> MOVE_NODE + -CHANGE_NODE_LABEL labelOld labelNew --> CHANGE_NODE_LABEL + -LOAD_GRAPH filename --> LOAD_GRAPH + -SAVE_GRAPH filename --> SAVE_GRAPH + + Giraff_Name/NavigationCommand + -StopGiraff --> CANCEL_NAVIGATION + -GoToNode label --> GET_PATH + -GoToPoint x y --> NAVIGATE_TARGET + -Relocalize x y --> RELOCALIZE_IN_AREA + -Motion v w --> MOTION_CMD_V + MOTION CMD_W + -RandomNavigator 0/1 --> RANDOM_NAVIGATOR + + Giraff_Name/MapBuildingCommand + -Rawlog Start/Stop --> SAVE_RAWLOG + + ---------------------------------------------------------------------------------------- + Giraff_Name/Localization <-- LOCALIZATION + Giraff_Name/Status <-- LOCALIZATION + Giraff_Name/ManualMode <-- CANCEL_NAVIGATION + Giraff_Name/Topology <-- GRAPH + Giraff_Name/ErrorMsg <-- ERROR_MSG + Giraff_Name/LaserScan <-- LASER1 + Giraff_Name/NavigationMode manual|ninguno <-- Navigation mode and destiny + auto|destino + recovery|destino + + Giraff_Name/Question msg <-- + Giraff_Name/Answer msg --> + + */ + +#include "CMQTTmosquitto.h" +#include "mosquitto.h" +#include +#include "Constants.h" + +#include +#include +#include +#include + +using namespace std; +using namespace mosqpp; +using namespace mrpt; +using namespace mrpt::utils; + +CMQTTMosquitto::CMQTTMosquitto(): mosquittopp( ("Giraff_"+mrpt::system::dateTimeToString(mrpt::system::now())).c_str() ,false) +{ +} + +CMQTTMosquitto::~CMQTTMosquitto() +{ +} + +bool CMQTTMosquitto::OnStartUp() +{ + cout << "Configuring MQTT connection..." << endl; + + // Read Module Parameters + //------------------------ + //! @moos_param localizationRate The rate (Hz) of sendign the localization to the Pilot client + localizationRate = m_ini.read_float("","localizationRate",10.0,false); + + //! @moos_param sendLaserScans Yes/No send laser scans to Pilot client + sendLaserScans = m_ini.read_bool("","sendLaserScans",true,false); + + //! @moos_param laserScanRate If sendLaserScans=yes, the rate (Hz) of sendign the laser scans + laserScanRate = m_ini.read_float("","laserScanRate",10.0,false); + + //! @moos_param laserScanResolution If sendLaserScans=yes, the resolution (%) of samples from the laser scan + laserScanResolution = m_ini.read_float("","laserScanResolution",100,false); + + cout << "READ: localizationRate = " << localizationRate << endl; + cout << "READ: sendLaserScans = " << sendLaserScans << endl; + cout << "READ: laserScanRate = " << laserScanRate << endl; + cout << "READ: laserScanResolution = " << laserScanResolution << endl << endl; + + + cout << "Setting Username and Password..." << endl; + // Read Connection Params + //------------------------- + //! @moos_param user The username to connect to the MQTT broker (new sesion). It is the asigned name to the Robot, e.g: Giraff_ES4 + broker_username = m_ini.read_string("","user","NO_USER_SET",true); + cout << "READ: user= " << broker_username << endl; + + //! @moos_param password The password to connect to the MQTT broker + broker_password = m_ini.read_string("","password","NO_PASSWORD_SET",false); + cout << "READ: password= " << broker_password << endl; + + cout << "Setting broker Host and Port..." << endl; + + std::string str_port; + //! @moos_param IP The IP address of the MQTT broker + if(m_MissionReader.GetConfigurationParam("IP",broker_host)) + { + //broker_host = broker_host_string.c_str(); + cout << "LOCAL MQTT Broker set on IP = " << broker_host << endl; + } + else + { + broker_host = Constants::host; + std::cout << "MQTT-IP default: " << broker_host << endl; + } + + //! @moos_param PORT The PORT of the MQTT broker + if(m_MissionReader.GetConfigurationParam("PORT", str_port)) + { + broker_port = atoi( str_port.c_str()); + std::cout << "LOCAL MQTT Broker set on PORT= " << broker_port << endl; + } + else + { + broker_port = Constants::port; + std::cout << "MQTT-PORT default: " << broker_port << endl; + } + + cout << "Setting security parameters..." << endl; + //Read Security Params + //--------------------- + //! @moos_param enable_TLS_security Use TLS security layer to connect to MQTT broker + enable_TLS_security = m_ini.read_bool("","enable_TLS_security",true,false); + + if( enable_TLS_security) + { + //! @moos_param TLS_path The path to the TLS certificates + TLS_path = m_ini.read_string("","TLS_path","./", /*Force existence:*/ false); + + //! @moos_param TLS_CAfile_pem Name of the CAfile + TLS_CAfile_pem = m_ini.read_string("","TLS_CAfile_pem","", /*Force existence:*/ true); + + //! @moos_param TLS_CERTfile_pem Name of the Certificate file + TLS_CERTfile_pem = m_ini.read_string("","TLS_CERTfile_pem","", /*Force existence:*/ true); + + //! @moos_param TLS_KEYfile_pem Name of the key file + TLS_KEYfile_pem = m_ini.read_string("","TLS_KEYfile_pem","", /*Force existence:*/ true); + } + + + + // Connect to MQTT broker + //----------------------- + //mosquittopp(broker_username.c_str(),false); //Initialize MQTT + + cout << "Establishing MQTT connection..." << endl; + int g = lib_init(); + if( username_pw_set(broker_username.c_str(), broker_password.c_str()) ) + std::cerr << "Error setting username and password.\n"; + + if (enable_TLS_security) + { + tlsOptionsSet(); //Set Security options and Data: tls-version, certificates... + cout << "Connecting to MQTT Broker with enabled TLS security" << endl; + } + else + { + cout << "Connecting to MQTT Broker with disabled TLS security" << endl; + } + + // Connect the Mosquitto Client + on_connect(connect(broker_host.c_str(), broker_port, Constants::keepalive_secs)); + //Subscribe to a list of topics + on_subscribe(NULL,2,NULL); + + + //Init timeStamps + timeStamp_last_loc = mrpt::system::now(); + timeStamp_last_laser = mrpt::system::now(); + + + is_motion_command_set = false; //indicates (true/false) if a motion command is active (To control Giraff from client) + return DoRegistrations(); + +} + +bool CMQTTMosquitto::OnCommandMsg( CMOOSMsg Msg ) +{ + if(Msg.IsSkewed(MOOSTime())) + return true; + + if(!Msg.IsString()) + return MOOSFail("pMQTTMosquitto only accepts string command messages\n"); + + std::string sCmd = Msg.GetString(); + + MOOSTrace("COMMAND RECEIVED: %s\n",sCmd.c_str()); + + return true; +} + +bool CMQTTMosquitto::Iterate() +{ + try + { + int rc = loop(-1); + if(rc!=MOSQ_ERR_SUCCESS) + { + //Connection lost with Broker! + std::cerr << "mosq_err " << rc << " - Reconnecting..." << endl; + int go = disconnect(); + int g = lib_cleanup(); + int gogo = lib_init(); + + if ( username_pw_set(broker_username.c_str(), broker_password.c_str()) ) + std::cerr << "Error setting username and password.\n"; + + if (enable_TLS_security) + { + tlsOptionsSet(); //Set Security options and Data: tls-version, certificates... + cout << "Connecting to MQTT Broker with enabled TLS security" << endl; + } + else + { + cout << "Connecting to MQTT Broker with disabled TLS security" << endl; + } + + /* Check Broker configuration + string str_ip; + if(m_MissionReader.GetConfigurationParam("IP",str_ip)) + { + broker_host = str_ip.c_str(); + cout << "LOCAL MQTT Broker set on IP = " << broker_host << endl; + } + else + { + broker_host = Constants::host; + std::cout << "MQTT-IP default: " << broker_host << endl; + } + */ + cout << "Connecting to MQTT Broker at IP: " << broker_host << ":"<< broker_port << endl; + // Connect the Mosquitto Client + on_connect(connect(broker_host.c_str(), broker_port, Constants::keepalive_secs)); + //Subscribe to a list of topics + on_subscribe(NULL,2,NULL); + + } + + //If a motion command (V,W) is active, check timeout + if (is_motion_command_set) + { + //compute time difference from last motion command + double seconds = mrpt::system::timeDifference(time_last_motion_command,mrpt::system::now()); + if (seconds > 0.2) + { + //Cancel motion + is_motion_command_set = false; + //! @moos_publish CANCEL_NAVIGATION Cancel the current navigation and return control to client (Pilot) + m_Comms.Notify("CANCEL_NAVIGATION", '1'); + } + } + + return true; + } + catch (std::exception &e) + { + return MOOSFail( (string("**ERROR**") + string(e.what())).c_str() ); + } + +} + + +bool CMQTTMosquitto::OnConnectToServer() +{ + // DoRegistrations(); + return true; +} + + +bool CMQTTMosquitto::DoRegistrations() +{ + //! @moos_subscribe LOCALIZATION + AddMOOSVariable( "LOCALIZATION", "LOCALIZATION", "LOCALIZATION", 0); + + //! @moos_subscribe GRAPH + AddMOOSVariable( "GRAPH", "GRAPH", "GRAPH", 0); + + //! @moos_subscribe ERROR_MSG + AddMOOSVariable( "ERROR_MSG", "ERROR_MSG", "ERROR_MSG", 0); + + //! @moos_subscribe CANCEL_NAVIGATION + AddMOOSVariable( "CANCEL_NAVIGATION", "CANCEL_NAVIGATION", "CANCEL_NAVIGATION", 0); + + //! @moos_subscribe LASER1 + AddMOOSVariable( "LASER1", "LASER1", "LASER1", 0); + + //! @moos_subscribe ROBOT_TOPOLOGICAL_PLACE + AddMOOSVariable( "ROBOT_TOPOLOGICAL_PLACE", "ROBOT_TOPOLOGICAL_PLACE", "ROBOT_TOPOLOGICAL_PLACE", 0); + + //! @moos_subscribe GIRAFF_CONTROL_MODE + AddMOOSVariable( "GIRAFF_CONTROL_MODE", "GIRAFF_CONTROL_MODE", "GIRAFF_CONTROL_MODE", 0); + + //! @moos_subscribe GET_PATH + AddMOOSVariable( "GET_PATH", "GET_PATH", "GET_PATH", 0); + + //! @moos_subscribe SHUTDOWN + AddMOOSVariable( "SHUTDOWN", "SHUTDOWN", "SHUTDOWN", 0); + + RegisterMOOSVariables(); + return true; +} + + +bool CMQTTMosquitto::OnNewMail(MOOSMSG_LIST &NewMail) +{ + string message=""; + for (MOOSMSG_LIST::iterator it=NewMail.begin();it!=NewMail.end();++it) + { + const CMOOSMsg &m = *it; + + //Send localization to Client via MQTT topic + // LOCALIZATION x y phi + if( it->GetName()=="LOCALIZATION") + { + if( mrpt::system::timeDifference(timeStamp_last_loc, mrpt::system::now()) > (1/localizationRate) ) + { + message=m.GetString(); // localization (x y phi) + + //Get aditionaly the Node Location (topological localization) + CMOOSVariable * pVar = GetMOOSVar("ROBOT_TOPOLOGICAL_PLACE"); + if(pVar) + message += "|" + pVar->GetStringVal(); + else + message += "|NULL"; + on_publish(NULL, (broker_username + "/" + "Localization").c_str(), strlen(message.c_str()), message.c_str()); + + + //Get aditionaly the Working Mode (GIRAFF_CONTROL_MODE: 0=Manual=Pilot, 2=Autonomous=OpenMORA) + CMOOSVariable * pVar2 = GetMOOSVar("GIRAFF_CONTROL_MODE"); + if(pVar2) + { + double current_mode = pVar2->GetDoubleVal(); + if( current_mode == 0.0) + message += "|Manual"; + else if(current_mode == 2.0) + message += "|Autonomous"; + else + message += "|Undefined"; + } + else + message += "|NULL"; + + on_publish(NULL, (broker_username + "/" + "Status").c_str(), strlen(message.c_str()), message.c_str()); + timeStamp_last_loc = mrpt::system::now(); + } + } + + //Inform the Client that the GRAPH has been updated + if( it->GetName()=="GRAPH") + { + cout << "Sending New GRAPH to Client" << endl; + std::string graph = "Graph"; + graph.append(it->GetString().c_str()); + on_publish(NULL,(broker_username + "/" + "Topology").c_str(),strlen(graph.c_str()),graph.c_str()); + } + + //Inform the Client of Errors + if( it->GetName()=="ERROR_MSG") + { + std::string graph = it->GetString().c_str(); + on_publish(NULL,(broker_username + "/" + "ErrorMsg").c_str(),strlen(graph.c_str()),graph.c_str()); + } + + //Inform Client about the current Navigation MODE + if( it->GetName()=="GIRAFF_CONTROL_MODE") + { + cout << "Sending Current Navigation Mode to Client: " << it->GetString() << endl; + size_t mode = (size_t)(it->GetDouble()); + + switch(mode) + { + case 0: //Manual mode + { + message = "manual|noDestiny"; + break; + } + case 2: //Autonomous mode + { + CMOOSVariable * pVar = GetMOOSVar("GET_PATH"); + std::string destiny = "NULL"; + if(pVar) + destiny = pVar->GetStringVal(); + message = "auto|" + destiny; + break; + } + default: + cout << "ERROR: Incorrect NavigationMode!!!!" << endl; + } + + on_publish(NULL, (broker_username + "/" + "NavigationMode").c_str(), strlen(message.c_str()), message.c_str()); + cout << endl << "NavigationMode is " << message.c_str() << endl; + } + + //Send Laser scans to client - 4Hz + if( it->GetName()=="LASER1") + { + if (sendLaserScans) + { + if( mrpt::system::timeDifference(timeStamp_last_laser,mrpt::system::now()) > (1/laserScanRate) ) + { + + CSerializablePtr obj; + mrpt::utils::RawStringToObject(it->GetString(),obj); + if (obj && IS_CLASS(obj,CObservation2DRangeScan)) + { + CObservation2DRangeScanPtr laser_obj = CObservation2DRangeScanPtr(obj); + size_t num_samples = laser_obj->scan.size(); + std::vector laser_readings; + + //Change resolution + size_t iter_inc = (size_t) (100/laserScanResolution); + + for (size_t i=0; iscan[i] ); + } + num_samples = laser_readings.size(); + + message = format( "Aperture %.2f#",laser_obj->aperture ); + message.append( format("Nsamples %u#",num_samples) ); + message.append("Values"); + for (size_t i=0; iRequestQuit(); + } + + UpdateMOOSVariables(NewMail); + return true; +} + + +/*--------------------------------------------------------------------------- + MQTT CallBack Methods +------------------------------------------------------------------------------*/ + +/*Additional method responsible for the csecurity configuration*/ +void CMQTTMosquitto::tlsOptionsSet() +{ + tls_insecure_set(true); + int r=tls_opts_set(1,"tlsv1"); + std::cout <<"TLS options:"<topic; + if (message->payloadlen) + std::cout << ", payload text is " << message->payload << "\n"; + else + std::cout << ", payload is (null)\n"; + + + char buf[51]; + memset(buf, 0, 51*sizeof(char)); + /* Copy N-1 bytes to ensure always 0 terminated. */ + memcpy(buf, message->payload, 50*sizeof(char)); + printf(buf, 50); + printf("\n"); + const char *aux; + aux=buf; + + + // TOPOLOGY-COMMANDS + if(!strcmp(message->topic, (broker_username + "/" +"TopologyCommand").c_str() )) + { + string topologyMessage = string(aux); + + //First element is the name of the action to perform in the topology graph + size_t pos = topologyMessage.find(" "); + std::string action = topologyMessage.substr(0, pos); + topologyMessage.erase(0, pos+1); + + if (action=="GetTopology") + { + CMOOSVariable *GRAPH = GetMOOSVar("GRAPH"); + std::string graph = "Graph"; + graph.append(GRAPH->GetStringVal().c_str()); + on_publish(NULL, (broker_username + "/" + "Topology").c_str(),strlen(graph.c_str()),graph.c_str()); + } + //ADD_NODE label type x y + else if(action == "ADD_NODE") + { + //! @moos_publish ADD_NODE Variable contaning the info of the new node to be added to the Graph (label type x y) + m_Comms.Notify(action, topologyMessage); + } + //REMOVE_NODE nodeLabel + else if(action == "REMOVE_NODE") + { + //! @moos_publish REMOVE_NODE Variable contaning the info of the node to be removed from the Graph (nodeLabel) + m_Comms.Notify(action, topologyMessage); + } + //ADD_ARC label_nodeA label_nodeB type + else if(action == "ADD_ARC") + { + //! @moos_publish ADD_ARC Variable contaning the info of the new arc to be added to the Graph (label_nodeA label_nodeB type) + m_Comms.Notify(action, topologyMessage); + } + //REMOVE_ARC nodeFrom nodeTo Arc_type + else if(action == "REMOVE_ARC") + { + //! @moos_publish REMOVE_ARC Variable contaning the info of the arc to be removed from the Graph (nodeFrom nodeTo Arc_type) + m_Comms.Notify(action, topologyMessage); + } + //MOVE_NODE label x y + else if(action == "MOVE_NODE") + { + //! @moos_publish MOVE_NODE Variable contaning the info of the new node position on the Graph (label x y) + m_Comms.Notify(action, topologyMessage); + } + //CHANGE_NODE_LABEL old_label new_label + else if(action == "CHANGE_NODE_LABEL") + { + //! @moos_publish CHANGE_NODE_LABEL Variable contaning the info to change the name of an existing node (old_label new_label) + m_Comms.Notify(action, topologyMessage); + } + //GET_NODE_POSITION node_label + else if(action == "GET_NODE_POSITION") + { + //! @moos_publish GET_NODE_POSITION Variable to request the location (x,y) of an existing node (node_label) + m_Comms.Notify(action, topologyMessage); + } + //LOAD_GRAPH file + else if(action == "LOAD_GRAPH") + { + //! @moos_publish LOAD_GRAPH Variable to request loading the topological graph from file (file) + m_Comms.Notify(action, topologyMessage); + } + //SAVE_GRAPH file + else if(action == "SAVE_GRAPH") + { + //! @moos_publish SAVE_GRAPH Variable to request saving the current topology to file (file) + m_Comms.Notify(action, topologyMessage); + } + } + + + // NAVIGATION-COMMANDS + else if(!strcmp(message->topic, (broker_username + "/" +"NavigationCommand").c_str() )) + { + string pluginCommand = string(aux); + + //First element is the name of the action to perform + size_t pos = pluginCommand.find(" "); + std::string action = pluginCommand.substr(0, pos); + pluginCommand.erase(0, pos+1); + + if(action == "StopGiraff") + { + //! @moos_publish CANCEL_NAVIGATION Cancel the current navigation and return control to client (Manual Mode) + cout << "Request received to cancel navigation" << endl; + m_Comms.Notify("CANCEL_NAVIGATION", '1'); + } + + // GoToPoint x y + if(action == "GoToPoint") + { + string str = format("Command for GoToPoint received at: %f",MOOSTime()); + MOOSDebugWrite(str); + + std::deque list; + mrpt::system::tokenize(pluginCommand," ",list); + if( list.size() >=2 ) + { + std::string gotox = list.at(0); + std::string gotoy = list.at(1); + const string target = format("[%.03f %.03f]", atof(gotox.c_str()),atof(gotoy.c_str())); + //! @moos_publish NAVIGATE_TARGET Set destination ([x y]) for Giraff to go autonomously + m_Comms.Notify("NAVIGATE_TARGET",target); + printf("%s\n",target.c_str()); + } + else + cout << "[MQTT Error]: GotoPoint command incorrect" << endl; + } + + // GoToNode label + else if(action == "GoToNode") + { + printf("Navegación solicitada a Nodo: %s\n",pluginCommand.c_str()); + + //! @moos_publish GO_TO_NODE Request the RobotController to start a reactive navigation to selected node. + m_Comms.Notify("GO_TO_NODE", pluginCommand ); + } + + // Relocalize x y + else if(action == "Relocalize") + { + //! @moos_publish RELOCALIZE_IN_AREA Request a robot relocalization arround designed point (x y) + m_Comms.Notify("RELOCALIZE_IN_AREA",pluginCommand); + } + + // Motion v w + else if(action == "Motion") + { + std::deque list; + mrpt::system::tokenize(pluginCommand," ",list); + if( list.size() >=2 ) + { + std::string setV = list.at(0); + std::string setW = list.at(1); + + //Get timeStamp + time_last_motion_command = mrpt::system::now(); + is_motion_command_set = true; + + //! @moos_publish GIRAFF_CONTROL_MODE The Giraff working mode: 0=Manual=Pilot, 2=Autonomous=OpenMORA + m_Comms.Notify("GIRAFF_CONTROL_MODE", 2); + //! @moos_publish MOTION_CMD_V Set robot linear speed + m_Comms.Notify("MOTION_CMD_V",atof(setV.c_str())); + //! @moos_publish MOTION_CMD_W Set robot angular speed + m_Comms.Notify("MOTION_CMD_W",atof(setW.c_str())); + } + else + cout << "[MQTT Error]: Motion command incorrect" << endl; + } + + // RandomNavigator 0/1 + else if(action == "RandomNavigator") + { + //! @moos_publish RANDOM_NAVIGATOR Activate/Deactivate the module to generate random navigations + m_Comms.Notify("RANDOM_NAVIGATOR",atof(pluginCommand.c_str()) ); + } + } + + // MAPBUILDING-COMMANDS + else if(!strcmp(message->topic, (broker_username + "/" +"MapBuildingCommand").c_str() )) + { + string command = string(aux); + + //First element is the name of the action to perform + size_t pos = command.find(" "); + std::string action = command.substr(0, pos); + command.erase(0, pos+1); + + // Rawlog Start/Stop + if(action == "Rawlog") + { + if( command == "Start") + { + saving_data = true; + command = "true"; + } + else if( command == "Stop") + { + saving_data=false; + command = "false"; + } + else + return; + + //! @moos_publish SAVE_RAWLOG Commands module Rawlog-grabber to start/stop saving data to rawlog file + m_Comms.Notify("SAVE_RAWLOG",command); + } + } + + // ClientACK + else if(!strcmp(message->topic, (broker_username + "/" +"ClientACK").c_str() )) + { + //! @moos_publish PILOT_MQTT_ACK Flag to indicate that communication with client is alive. + m_Comms.Notify("PILOT_MQTT_ACK","1"); + } +} + + +void CMQTTMosquitto::on_subscribe(uint16_t mid, int qos_count, const uint8_t *granted_qos) +{ + for (int i = 0; i < Constants::topic_count; ++i) + { + std::string GirafName_topic = broker_username + "/" + Constants::topics[i]; + std::cerr << "Subscribing to " << GirafName_topic << "\n"; + int rc=subscribe(NULL, GirafName_topic.c_str(), qos_count); + if (rc) + std::cerr << "Error: failed to subscribe to " << GirafName_topic << ", subscribe returned " << rc << "\n"; + else + std::cerr << "Subscribed to " << GirafName_topic << ", OK!\n"; + } + time_t rawtime; + time(&rawtime); + std::stringstream strm; + strm << "MQTT topics subscription done: " << ctime(&rawtime) << endl; +} + +void CMQTTMosquitto::on_publish(int *mid, const char *topic, int payloadlen, const void *payload) +{ + int n= publish(mid,topic,payloadlen,payload); +} \ No newline at end of file diff --git a/MQTTMosquitto/CMQTTMosquitto.h b/MQTTMosquitto/CMQTTMosquitto.h new file mode 100644 index 0000000..a9f355f --- /dev/null +++ b/MQTTMosquitto/CMQTTMosquitto.h @@ -0,0 +1,92 @@ +/* +---------------------------------------------------------------------------+ + | Open MORA (MObile Robot Arquitecture) | + | | + | http://babel.isa.uma.es/mora/ | + | | + | Copyright (C) 2010 University of Malaga | + | | + | This software was written by the Machine Perception and Intelligent | + | Robotics (MAPIR) Lab, University of Malaga (Spain). | + | Contact: Jose-Luis Blanco | + | | + | This file is part of the MORA project. | + | | + | MORA is free software: you can redistribute it and/or modify | + | it under the terms of the GNU General Public License as published by | + | the Free Software Foundation, either version 3 of the License, or | + | (at your option) any later version. | + | | + | MORA is distributed in the hope that it will be useful, | + | but WITHOUT ANY WARRANTY; without even the implied warranty of | + | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | + | GNU General Public License for more details. | + | | + | You should have received a copy of the GNU General Public License | + | along with MORA. If not, see . | + | | + +---------------------------------------------------------------------------+ */ + +#ifndef CMQTTMosquitto_H +#define CMQTTMosquitto_H + +#include "Constants.h" +#include +#include +#include + +/*#include + +#include +#include + +#include "CGiraffCommunication.h" +#include "CGiraffMotorsCom.h"*/ +using namespace std; +using namespace mosqpp; + +class CMQTTMosquitto :public COpenMORAApp,mosquittopp +{ + /** Class for storing an Hokuyo observation. */ + +public: + CMQTTMosquitto(); + virtual ~CMQTTMosquitto(); + /////////////////// CHokuyoObservation /////////////////// + bool saving_data; + double lin_vel,ang_vel; +protected: + //const char *client_id; + //const char *broker_host; + std::string broker_host; + int broker_port; + std::string broker_username, broker_password; + /** called at startup */ + virtual bool OnStartUp(); + /** called when new mail arrives */ + virtual bool OnNewMail(MOOSMSG_LIST & NewMail); + /** called when work is to be done */ + virtual bool Iterate(); + /** called when app connects to DB */ + virtual bool OnConnectToServer(); + + bool OnCommandMsg( CMOOSMsg Msg ); + + /** performs the registration for mail */ + bool DoRegistrations(); + +private: + void tlsOptionsSet(); + void on_connect(int rc); + void on_message(const struct mosquitto_message *message); + void on_subscribe(uint16_t mid, int qos_count, const uint8_t *granted_qos); + void on_publish(int *mid, const char *topic, int payloadlen, const void *payload); + + mrpt::system::TTimeStamp timeStamp_last_loc, timeStamp_last_laser; + bool sendLaserScans, enable_TLS_security; + float laserScanResolution, laserScanRate, localizationRate; + std::string TLS_path, TLS_CAfile_pem, TLS_CERTfile_pem, TLS_KEYfile_pem; + bool is_motion_command_set; + mrpt::system::TTimeStamp time_last_motion_command; +}; + +#endif diff --git a/MQTTMosquitto/CMakeLists.txt b/MQTTMosquitto/CMakeLists.txt new file mode 100644 index 0000000..6f2d243 --- /dev/null +++ b/MQTTMosquitto/CMakeLists.txt @@ -0,0 +1,26 @@ +PROJECT(MQTTMosquitto) + +# Use MRPT: +FIND_PACKAGE( MRPT REQUIRED slam gui hwdrivers) + +INCLUDE_DIRECTORIES( ${MOOS_INCLUDE_DIRS} ) +LINK_DIRECTORIES( ${MOOS_LIBRARY_DIRS} ) + + +#Use Mosquitto libraries +find_path(MQTTmosquitto_LIB NAMES mosquitto.lib mosquittopp.lib DOC "Path to the folder containing the MQTTmosquitto mosquitto.lib and mosquittopp.lib libraries") +if (${MQTTmosquitto_LIB} STREQUAL "MQTTmosquitto_LIB-NOTFOUND") + message ("MQTTmosquitto 'mosquitto.lib' or 'mosquittopp.lib' not Found!. \nPlease, provide the path to the MQTTmosquitto libraries.") +endif (${MQTTmosquitto_LIB} STREQUAL "MQTTmosquitto_LIB-NOTFOUND") + +INCLUDE_DIRECTORIES(${MQTTmosquitto_LIB}) + +# Add executable +ADD_EXECUTABLE( ${PROJECT_NAME} + main.cpp + CMQTTMosquitto.cpp CMQTTMosquitto.h + Constants.cpp Constants.h + ) + +# Link the executable +TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${MOOS_LIBRARIES} ${MRPT_LIBS} "${MQTTmosquitto_LIB}/mosquitto.lib" "${MQTTmosquitto_LIB}/mosquittopp.lib" ) diff --git a/MQTTMosquitto/Constants.cpp b/MQTTMosquitto/Constants.cpp new file mode 100644 index 0000000..d387aa0 --- /dev/null +++ b/MQTTMosquitto/Constants.cpp @@ -0,0 +1,9 @@ +#include "Constants.h" + +// see constants.h for info +const char *Constants::host = "giraffplus.xlab.si"; +const int Constants::port = 8883; +const int Constants::keepalive_secs = 30; +const char *Constants::topics[] = {"TopologyCommand","NavigationCommand","MapBuildingCommand","ClientACK"}; +const int Constants::topic_count = (sizeof(Constants::topics) / sizeof(char*)); +const char *Constants::publish_topic = "hello_topic"; \ No newline at end of file diff --git a/MQTTMosquitto/Constants.h b/MQTTMosquitto/Constants.h new file mode 100644 index 0000000..f10706c --- /dev/null +++ b/MQTTMosquitto/Constants.h @@ -0,0 +1,13 @@ +#pragma once +class Constants { +public: + /* To find your MQTT Username and Password + login into your ZADATA account and click + navbar -> "Settings" -> "Credentials" */ + const static char *host; // host for MQTT server + const static int port; // MQTT port + const static int keepalive_secs; // keepalive ping interval in seconds + const static char *topics[]; // topics to subscribe to + const static int topic_count; // number of topics in topics array + const static char *publish_topic; // topic to publish hello message to +}; \ No newline at end of file diff --git a/MQTTMosquitto/main.cpp b/MQTTMosquitto/main.cpp new file mode 100644 index 0000000..528dd0f --- /dev/null +++ b/MQTTMosquitto/main.cpp @@ -0,0 +1,51 @@ +/* +---------------------------------------------------------------------------+ + | Open MORA (MObile Robot Arquitecture) | + | | + | http://babel.isa.uma.es/mora/ | + | | + | Copyright (C) 2010 University of Malaga | + | | + | This software was written by the Machine Perception and Intelligent | + | Robotics (MAPIR) Lab, University of Malaga (Spain). | + | Contact: Jose-Luis Blanco | + | | + | This file is part of the MORA project. | + | | + | MORA is free software: you can redistribute it and/or modify | + | it under the terms of the GNU General Public License as published by | + | the Free Software Foundation, either version 3 of the License, or | + | (at your option) any later version. | + | | + | MORA is distributed in the hope that it will be useful, | + | but WITHOUT ANY WARRANTY; without even the implied warranty of | + | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | + | GNU General Public License for more details. | + | | + | You should have received a copy of the GNU General Public License | + | along with MORA. If not, see . | + | | + +---------------------------------------------------------------------------+ */ + + +#include "CMQTTMosquitto.h" + +int main(int argc ,char * argv[]) +{ + const char * sMissionFile = "Mission.moos"; + const char * sMOOSName = "pMobileRobot_Giraff"; + + + switch(argc) + { + case 3: + sMOOSName = argv[2]; + case 2: + sMissionFile = argv[1]; + } + + CMQTTMosquitto TheApp; + TheApp.Run(sMOOSName,sMissionFile); + + return 0; +} + diff --git a/SpeechSynth/CMakeLists.txt b/SpeechSynth/CMakeLists.txt new file mode 100644 index 0000000..cc2bda3 --- /dev/null +++ b/SpeechSynth/CMakeLists.txt @@ -0,0 +1,16 @@ +PROJECT(SpeechSynth) + +# Use MRPT: +FIND_PACKAGE( MRPT REQUIRED gui ) # hmtslam hwdrivers ... any other sub-library + +INCLUDE_DIRECTORIES( ${MOOS_INCLUDE_DIRS} ) +LINK_DIRECTORIES( ${MOOS_LIBRARY_DIRS} ) + +# Add executable +ADD_EXECUTABLE( ${PROJECT_NAME} + main.cpp + CSpeechSynth.cpp CSpeechSynth.hpp + ) + +# Link the executable +TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${MOOS_LIBRARIES} ${MRPT_LIBS} ) diff --git a/SpeechSynth/CSpeechSynth.cpp b/SpeechSynth/CSpeechSynth.cpp new file mode 100644 index 0000000..2b48655 --- /dev/null +++ b/SpeechSynth/CSpeechSynth.cpp @@ -0,0 +1,138 @@ +/* +---------------------------------------------------------------------------+ + | Open MORA (MObile Robot Arquitecture) | + | | + | http://babel.isa.uma.es/mora/ | + | | + | Copyright (C) 2010 University of Malaga | + | | + | This software was written by the Machine Perception and Intelligent | + | Robotics (MAPIR) Lab, University of Malaga (Spain). | + | Contact: Jose-Luis Blanco | + | | + | This file is part of the MORA project. | + | | + | MORA is free software: you can redistribute it and/or modify | + | it under the terms of the GNU General Public License as published by | + | the Free Software Foundation, either version 3 of the License, or | + | (at your option) any later version. | + | | + | MORA is distributed in the hope that it will be useful, | + | but WITHOUT ANY WARRANTY; without even the implied warranty of | + | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | + | GNU General Public License for more details. | + | | + | You should have received a copy of the GNU General Public License | + | along with MORA. If not, see . | + | | + +---------------------------------------------------------------------------+ */ + +/** @moos_module A module to keep the high-level tasks commanded to the robot. + * + */ + +#include "CSpeechSynth.hpp" + +#include +#include +#include + +using namespace std; + +using namespace mrpt; +using namespace mrpt::utils; +//using namespace .... + + +CSpeechSynthApp::CSpeechSynthApp() +// : var (init_val), ... +{ +} + +CSpeechSynthApp::~CSpeechSynthApp() +{ +} + +bool CSpeechSynthApp::OnStartUp() +{ + // Read parameters (if any) from the mission configuration file. + //! @moos_param PARAM_NAME PARAM DESCRIPTION IN ONE LINE + //m_MissionReader.GetConfigurationParam("my_param",m_myvar); + + // There is also a MRPT-like object (this->m_ini) that is a wrapper + // to read from the module config block of the current MOOS mission file. + // m_ini.read_int(...); + return DoRegistrations(); +} + +bool CSpeechSynthApp::Iterate() +{ + // Are there + + + return true; +} + +bool CSpeechSynthApp::OnConnectToServer() +{ + DoRegistrations(); + return true; +} + +bool CSpeechSynthApp::OnCommandMsg( CMOOSMsg Msg ) +{ + if(Msg.IsSkewed(MOOSTime())) return true; + if(!Msg.IsString()) return MOOSFail("This module only accepts string command messages\n"); + const std::string sCmd = Msg.GetString(); + //MOOSTrace("COMMAND RECEIVED: %s\n",sCmd.c_str()); + // Process the command "sCmd". + return true; +} + +bool CSpeechSynthApp::DoRegistrations() +{ + //! @moos_subscribe NEW_TASK + AddMOOSVariable("SAY","SAY","SAY",0); + + RegisterMOOSVariables(); + return true; +} + + +bool CSpeechSynthApp::OnNewMail(MOOSMSG_LIST &NewMail) +{ + std::string cad; + for(MOOSMSG_LIST::iterator i=NewMail.begin();i!=NewMail.end();++i) + { + + if (i->GetName()=="SAY") + { + + MOOSTrace("Received talk: %s\n",i->GetString().c_str()); + + const size_t len = strlen(i->GetString().c_str()) + 1; + HGLOBAL hMem = GlobalAlloc(GMEM_MOVEABLE, len); + memcpy(GlobalLock(hMem), i->GetString().c_str(), len); + GlobalUnlock(hMem); + OpenClipboard(0); + EmptyClipboard(); + SetClipboardData(CF_TEXT, hMem); + CloseClipboard(); + + // Is it necessary to free the memory allocated???? + // GlobalFree(hMem); + + + } + + if( (i->GetName()=="SHUTDOWN") && (MOOSStrCmp(i->GetString(),"true")) ) + { + // Disconnect comms: + MOOSTrace("Closing Module \n"); + this->RequestQuit(); + } + + } + + UpdateMOOSVariables(NewMail); + return true; +} diff --git a/SpeechSynth/CSpeechSynth.hpp b/SpeechSynth/CSpeechSynth.hpp new file mode 100644 index 0000000..422331a --- /dev/null +++ b/SpeechSynth/CSpeechSynth.hpp @@ -0,0 +1,58 @@ +/* +---------------------------------------------------------------------------+ + | Open MORA (MObile Robot Arquitecture) | + | | + | http://babel.isa.uma.es/mora/ | + | | + | Copyright (C) 2010 University of Malaga | + | | + | This software was written by the Machine Perception and Intelligent | + | Robotics (MAPIR) Lab, University of Malaga (Spain). | + | Contact: Jose-Luis Blanco | + | | + | This file is part of the MORA project. | + | | + | MORA is free software: you can redistribute it and/or modify | + | it under the terms of the GNU General Public License as published by | + | the Free Software Foundation, either version 3 of the License, or | + | (at your option) any later version. | + | | + | MORA is distributed in the hope that it will be useful, | + | but WITHOUT ANY WARRANTY; without even the implied warranty of | + | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | + | GNU General Public License for more details. | + | | + | You should have received a copy of the GNU General Public License | + | along with MORA. If not, see . | + | | + +---------------------------------------------------------------------------+ */ + +#ifndef CSpeechSynth_H +#define CSpeechSynth_H + +#include +#include + +class CSpeechSynthApp : public CMOOSApp +{ +public: + CSpeechSynthApp(); + virtual ~CSpeechSynthApp(); + +protected: + /** called at startup */ + virtual bool OnStartUp(); + /** called when new mail arrives */ + virtual bool OnNewMail(MOOSMSG_LIST & NewMail); + /** called when work is to be done */ + virtual bool Iterate(); + /** called when app connects to DB */ + virtual bool OnConnectToServer(); + + bool OnCommandMsg( CMOOSMsg Msg ); + + bool DoRegistrations(); + + + +}; +#endif diff --git a/SpeechSynth/main.cpp b/SpeechSynth/main.cpp new file mode 100644 index 0000000..56bd447 --- /dev/null +++ b/SpeechSynth/main.cpp @@ -0,0 +1,46 @@ +/* +---------------------------------------------------------------------------+ + | Open MORA (MObile Robot Arquitecture) | + | | + | http://babel.isa.uma.es/mora/ | + | | + | Copyright (C) 2010 University of Malaga | + | | + | This software was written by the Machine Perception and Intelligent | + | Robotics (MAPIR) Lab, University of Malaga (Spain). | + | Contact: Jose-Luis Blanco | + | | + | This file is part of the MORA project. | + | | + | MORA is free software: you can redistribute it and/or modify | + | it under the terms of the GNU General Public License as published by | + | the Free Software Foundation, either version 3 of the License, or | + | (at your option) any later version. | + | | + | MORA is distributed in the hope that it will be useful, | + | but WITHOUT ANY WARRANTY; without even the implied warranty of | + | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | + | GNU General Public License for more details. | + | | + | You should have received a copy of the GNU General Public License | + | along with MORA. If not, see . | + | | + +---------------------------------------------------------------------------+ */ + +#include "CSpeechSynth.hpp" + +int main(int argc ,char * argv[]) +{ + const char * sMissionFile = "Mission.moos"; + const char * sMOOSName = "pSpeech_Synth"; + switch(argc) + { + case 3: + sMOOSName = argv[2]; + case 2: + sMissionFile = argv[1]; + } + + CSpeechSynthApp TheApp; + TheApp.Run(sMOOSName,sMissionFile); + return 0; +}