Difference between revisions of "David-PFC-ROS3"

cd 
mkdir ros_workspace
echo "ROS_PACKAGE_PATH=~/ros:$ROS_PACKAGE_PATH" >> ~/.bashrc
echo "ROS_WORKSPACE=~/ros" >> ~/.bashrc

sudo aptitude install qtcreator

/**
 * @file /src/main_window.cpp
 *
 * @brief Implementation for the qt gui.
 *
 * @date February 2011
 **/
/*****************************************************************************
** Includes
*****************************************************************************/

#include <QtGui>
#include <QMessageBox>
#include <iostream>
#include <QString>
#include "../include/qtTurtlebot/main_window.hpp"
#include "../include/qtTurtlebot/turtlebotTeleop.h"

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <signal.h>
#include <termios.h>
#include <stdio.h>
#include "boost/thread/mutex.hpp"
#include "boost/thread/thread.hpp"

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace qtTurtlebot {

using namespace Qt;

/*****************************************************************************
** Implementation [MainWindow]
*****************************************************************************/

MainWindow::MainWindow(int argc, char** argv, QWidget *parent)
	: QMainWindow(parent)
	, qnode(argc,argv)
{
	ui.setupUi(this); // Calling this incidentally connects all ui's triggers to on_...() callbacks in this class.
    QObject::connect(ui.actionAbout_Qt, SIGNAL(triggered(bool)), qApp, SLOT(aboutQt())); // qApp is a global variable for the application

    ReadSettings();
	setWindowIcon(QIcon(":/images/icon.png"));
	ui.tab_manager->setCurrentIndex(0); // ensure the first tab is showing - qt-designer should have this already hardwired, but often loses it (settings?).
    QObject::connect(&qnode, SIGNAL(rosShutdown()), this, SLOT(close()));

	/*********************
	** Logging
	**********************/
	ui.view_logging->setModel(qnode.loggingModel());
    QObject::connect(&qnode, SIGNAL(loggingUpdated()), this, SLOT(updateLoggingView()));

    /*********************
    ** Auto Start
    **********************/
    if ( ui.checkbox_remember_settings->isChecked() ) {
        on_button_connect_clicked(true);
    }

    //Conectando la señal de los botones a los slots apropiados
    QObject::connect(ui.botonArriba, SIGNAL(clicked()),this, SLOT(botonArriba()));
    QObject::connect(ui.botonDerecha, SIGNAL(clicked()),this, SLOT(botonDerecha()));
    QObject::connect(ui.botonAbajo, SIGNAL(clicked()),this, SLOT(botonAbajo()));
    QObject::connect(ui.botonIzquierda, SIGNAL(clicked()),this, SLOT(botonIzquierda()));

}

MainWindow::~MainWindow() {}

/*****************************************************************************
** Implementation [Slots]
*****************************************************************************/


void MainWindow::showNoMasterMessage() {
	QMessageBox msgBox;
	msgBox.setText("Couldn't find the ros master.");
	msgBox.exec();
    close();
}

/*
 * These triggers whenever the button is clicked, regardless of whether it
 * is already checked or not.
 */

void MainWindow::on_button_connect_clicked(bool check ) {
	if ( ui.checkbox_use_environment->isChecked() ) {
		if ( !qnode.init() ) {
			showNoMasterMessage();
		} else {
			ui.button_connect->setEnabled(false);
		}
	} else {
		if ( ! qnode.init(ui.line_edit_master->text().toStdString(),
				   ui.line_edit_host->text().toStdString()) ) {
			showNoMasterMessage();
		} else {
			ui.button_connect->setEnabled(false);
			ui.line_edit_master->setReadOnly(true);
			ui.line_edit_host->setReadOnly(true);
			ui.line_edit_topic->setReadOnly(true);
		}
	}
}


void MainWindow::on_checkbox_use_environment_stateChanged(int state) {
	bool enabled;
	if ( state == 0 ) {
		enabled = true;
	} else {
		enabled = false;
	}
	ui.line_edit_master->setEnabled(enabled);
	ui.line_edit_host->setEnabled(enabled);
	//ui.line_edit_topic->setEnabled(enabled);
}

/*****************************************************************************
** Implemenation [Slots][manually connected]
*****************************************************************************/

/**
 * This function is signalled by the underlying model. When the model changes,
 * this will drop the cursor down to the last line in the QListview to ensure
 * the user can always see the latest log message.
 */
void MainWindow::updateLoggingView() {
        ui.view_logging->scrollToBottom();
}

/*****************************************************************************
** Implementation [Menu]
*****************************************************************************/

void MainWindow::on_actionAbout_triggered() {
    QMessageBox::about(this, tr("About ..."),tr("<h2>PACKAGE_NAME Test Program 0.10</h2><p>Copyright Yujin Robot</p><p>This package needs an about description.</p>"));
}




void MainWindow::handleButton(){
    QMessageBox::about(this, tr("MENSAJE  ..."),tr("<h2>WARNING</h2><p>Copyright Yujin Robot</p><p>This package needs an about description.</p>"));
}

void MainWindow::botonAbajo(){
    qnode.publicar(0,-1.0);
}

void MainWindow::botonArriba(){
    qnode.publicar(0,1.0);
}

void MainWindow::botonDerecha(){
    qnode.publicar(-1.0,0);
}

void MainWindow::botonIzquierda(){
    qnode.publicar(1.0,0);
}

/*****************************************************************************
** Implementation [Configuration]
*****************************************************************************/

void MainWindow::ReadSettings() {
    QSettings settings("Qt-Ros Package", "qtTurtlebot");
    restoreGeometry(settings.value("geometry").toByteArray());
    restoreState(settings.value("windowState").toByteArray());
    QString master_url = settings.value("master_url",QString("http://192.168.1.2:11311/")).toString();
    QString host_url = settings.value("host_url", QString("192.168.1.3")).toString();
    //QString topic_name = settings.value("topic_name", QString("/chatter")).toString();
    ui.line_edit_master->setText(master_url);
    ui.line_edit_host->setText(host_url);
    //ui.line_edit_topic->setText(topic_name);
    bool remember = settings.value("remember_settings", false).toBool();
    ui.checkbox_remember_settings->setChecked(remember);
    bool checked = settings.value("use_environment_variables", false).toBool();
    ui.checkbox_use_environment->setChecked(checked);
    if ( checked ) {
    	ui.line_edit_master->setEnabled(false);
    	ui.line_edit_host->setEnabled(false);
    	//ui.line_edit_topic->setEnabled(false);
    }
}

void MainWindow::WriteSettings() {
    QSettings settings("Qt-Ros Package", "qtTurtlebot");
    settings.setValue("master_url",ui.line_edit_master->text());
    settings.setValue("host_url",ui.line_edit_host->text());
    //settings.setValue("topic_name",ui.line_edit_topic->text());
    settings.setValue("use_environment_variables",QVariant(ui.checkbox_use_environment->isChecked()));
    settings.setValue("geometry", saveGeometry());
    settings.setValue("windowState", saveState());
    settings.setValue("remember_settings",QVariant(ui.checkbox_remember_settings->isChecked()));

}

void MainWindow::closeEvent(QCloseEvent *event)
{
	WriteSettings();
	QMainWindow::closeEvent(event);
}

}  // namespace qtTurtlebot

/**
* @file /src/qnode.cpp
*
* @brief Ros communication central!
*
* @date February 2011
**/

/*****************************************************************************
** Includes
*****************************************************************************/

#include <ros/ros.h>
#include <ros/network.h>
#include <string>
#include <std_msgs/String.h>
#include <sstream>
#include "../include/qtTurtlebot/qnode.hpp"

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace qtTurtlebot {

/*****************************************************************************
** Implementation
*****************************************************************************/

QNode::QNode(int argc, char** argv ) :
init_argc(argc),
init_argv(argv)
{}

QNode::~QNode() {
if(ros::isStarted()) {
  ros::shutdown(); // explicitly needed since we use ros::start();
  ros::waitForShutdown();
}
wait();
}

bool QNode::init() {
ros::init(init_argc,init_argv,"qtTurtlebot");
if ( ! ros::master::check() ) {
    return false;
}
ros::start(); // explicitly needed since our nodehandle is going out of scope.
ros::NodeHandle n, nh_, ph_;

// Add your ros communications here.

//Inicializando variables de turtlebotTeleop
//ph_ = "~";
linear_ = 0;
angular_ = 0;
l_scale_ = 1.0;
a_scale_ = 1.0;

ph_.param("scale_angular", a_scale_, a_scale_);
ph_.param("scale_linear", l_scale_, l_scale_);

vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1);

//Resto del main de turtlebotTeleop
//signal(SIGINT,quit);

//boost::thread my_thread(boost::bind(&QNode::keyLoop, &QNode));

//ros::Timer timer = n.createTimer(ros::Duration(0.1), boost::bind(&QNode::watchdog, &QNode));

//my_thread.interrupt() ;
//my_thread.join() ;

start();
return true;
}

bool QNode::init(const std::string &master_url, const std::string &host_url) {
std::map<std::string,std::string> remappings;
remappings["__master"] = master_url;
remappings["__hostname"] = host_url;
ros::init(remappings,"qtTurtlebot");
if ( ! ros::master::check() ) {
    return false;
}
ros::start(); // explicitly needed since our nodehandle is going out of scope.
ros::NodeHandle n, nh_, ph_;
// Add your ros communications here.

//Inicializando variables de turtlebotTeleop
//ph_ = "_";
linear_ = 0;
angular_ = 0;
l_scale_ = 1.0;
a_scale_ = 1.0;

ph_.param("scale_angular", a_scale_, a_scale_);
ph_.param("scale_linear", l_scale_, l_scale_);

vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1);

//Resto del main de turtlebotTeleop
//signal(SIGINT,quit);

//boost::thread my_thread(boost::bind(&QNode::keyLoop, &QNode));

//ros::Timer timer = n.createTimer(ros::Duration(0.1), boost::bind(&QNode::watchdog, &QNode));

//my_thread.interrupt() ;
//my_thread.join() ;

start();
return true;
}

void QNode::run() {
ros::Rate loop_rate(1);
//int count = 0;
while ( ros::ok() ) {

   // std_msgs::String msg;
   // std::stringstream ss;
   // ss << "hello world " << count;
    //msg.data = ss.str();
    //chatter_publisher.publish(msg);
    //log(Info,std::string("I sent: ")+msg.data);
    ros::spinOnce();
    loop_rate.sleep();
    //++count;
}
std::cout << "Ros shutdown, proceeding to close the gui." << std::endl;
Q_EMIT rosShutdown(); // used to signal the gui for a shutdown (useful to roslaunch)
}


void QNode::log( const LogLevel &level, const std::string &msg) {
logging_model.insertRows(logging_model.rowCount(),1);
std::stringstream logging_model_msg;
switch ( level ) {
    case(Debug) : {
            ROS_DEBUG_STREAM(msg);
            logging_model_msg << "[DEBUG] [" << ros::Time::now() << "]: " << msg;
            break;
    }
    case(Info) : {
            ROS_INFO_STREAM(msg);
            logging_model_msg << "[INFO] [" << ros::Time::now() << "]: " << msg;
            break;
    }
    case(Warn) : {
            ROS_WARN_STREAM(msg);
            logging_model_msg << "[INFO] [" << ros::Time::now() << "]: " << msg;
            break;
    }
    case(Error) : {
            ROS_ERROR_STREAM(msg);
            logging_model_msg << "[ERROR] [" << ros::Time::now() << "]: " << msg;
            break;
    }
    case(Fatal) : {
            ROS_FATAL_STREAM(msg);
            logging_model_msg << "[FATAL] [" << ros::Time::now() << "]: " << msg;
            break;
    }
}
QVariant new_row(QString(logging_model_msg.str().c_str()));
logging_model.setData(logging_model.index(logging_model.rowCount()-1),new_row);
Q_EMIT loggingUpdated(); // used to readjust the scrollbar
}

void QNode::publicar(double angular, double linear){
    publish(angular, linear);
}

void QNode::publish(double angular, double linear)
{
    geometry_msgs::Twist vel;
    vel.angular.z = a_scale_*angular;
    vel.linear.x = l_scale_*linear;

    vel_pub_.publish(vel);


  return;
}


}  // namespace qtTurtlebot