VelocityDispatcher.cpp

Go to the documentation of this file.

#include <iostream>
#include <boost/thread.hpp>
#include <boost/foreach.hpp>

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Int32.h>

using namespace std;
using namespace ros;

#define foreach BOOST_FOREACH

/*************************************************************************************************
*** Parameters
**************************************************************************************************/

volatile bool _enableOutput;

int _currentInput;
int _currentOutput;

vector<Subscriber> _inputs;
vector<Publisher>  _outputs;

Subscriber _setInputSubscriber;
Subscriber _setOutputSubscriber;

Publisher _currentInputPublisher;
Publisher _currentOutputPublisher;

boost::thread* _velocityPublishThread;
Timer _statusPublishTimer;

geometry_msgs::Twist _currentVelocity;

/*************************************************************************************************
*** Callbacks
**************************************************************************************************/

void onSetInputMessage(const std_msgs::Int32::ConstPtr inputIdMessage) {
        if (inputIdMessage->data >= 1 && inputIdMessage->data <= _inputs.size()) {
                _currentInput = inputIdMessage->data - 1;
                ROS_INFO("Current input changed to '#%i' ['%s']", inputIdMessage->data, _inputs[_currentInput].getTopic().c_str());
        }
}

void onSetOutputMessage(const std_msgs::Int32::ConstPtr outputIdMessage) {
        if (outputIdMessage->data >= 1 && outputIdMessage->data <= _outputs.size()) {
                _currentOutput = outputIdMessage->data - 1;
                _enableOutput = true;
                ROS_INFO("Current output changed to '#%i' ['%s']", outputIdMessage->data, _outputs[_currentOutput].getTopic().c_str());
        }

        if (outputIdMessage->data == 0) {
                _enableOutput = false;
                ROS_INFO("Current output changed to NONE [Disabled]");
        }
}

void onInputVelocityMessage(const geometry_msgs::Twist::ConstPtr velocityMessage, int inputId) {
        if (inputId == _currentInput + 1)
                _currentVelocity = *velocityMessage;
}

/*************************************************************************************************
*** Functions
**************************************************************************************************/

void publishStatus(TimerEvent timerEvent) {
        std_msgs::Int32 intMessage;

        intMessage.data = _currentInput + 1;
        _currentInputPublisher.publish(intMessage);

        intMessage.data = _currentOutput + 1;
        _currentOutputPublisher.publish(intMessage);
}

void publishVelocity() {
        while (ros::ok()) {

                if (_enableOutput)
                        _outputs[_currentOutput].publish(_currentVelocity);

                boost::this_thread::sleep(boost::posix_time::milliseconds(1000.0 / 20.0));
        }
}

void createSubscribersAndPublishers(NodeHandle& node, map<string, string>& inputs, vector<string>& outputs) {
        int inputId = 1;
        ROS_INFO("[Preparing inputs]");
        pair<string, string> input;
        foreach (input, inputs) {
                ROS_INFO(" - Setting up input topic #%i: %s", inputId, input.second.c_str());

                _inputs.push_back(node.subscribe<geometry_msgs::Twist>
                        (input.second, 100, boost::bind(onInputVelocityMessage, _1, inputId++)));
        }

        int outputId = 1;
        ROS_INFO("[Preparing outputs]");
        foreach (string output, outputs) {
                ROS_INFO(" - Setting up output topic #%i: %s", outputId++, output.c_str());

                _outputs.push_back(node.advertise<geometry_msgs::Twist>(output, 1, false));
        }

        _setInputSubscriber = node.subscribe("/velocity_dispatcher/set_input", 1, onSetInputMessage);
        _setOutputSubscriber = node.subscribe("/velocity_dispatcher/set_output", 1, onSetOutputMessage);

        _currentInputPublisher = node.advertise<std_msgs::Int32>("/velocity_dispatcher/current_input", 1, true);
        _currentOutputPublisher = node.advertise<std_msgs::Int32>("/velocity_dispatcher/current_output", 1, true);

        _statusPublishTimer = node.createTimer(Duration(1), boost::bind(publishStatus, _1));
        _velocityPublishThread = new boost::thread(publishVelocity);
}

bool setupInputOutput(NodeHandle& node) {

        map<string, string> inputTopics;
        vector<string> outputTopics;

        if (node.hasParam("/mr_teleoperator/input_topics"))
                node.getParam("/mr_teleoperator/input_topics", inputTopics);
        else {
                ROS_ERROR("No input topics provided");
                return false;
        }

        if (node.hasParam("/mr_teleoperator/output_topics"))
                node.getParam("/mr_teleoperator/output_topics", outputTopics);
        else {
                ROS_ERROR("No output topics provided");
                return false;
        }

        _enableOutput   = true;
        _currentInput   = 0;
        _currentOutput  = 0;

        createSubscribersAndPublishers(node, inputTopics, outputTopics);

        return true;

}

/*************************************************************************************************
*** THIS IS MAAAAAAAAAAAAAAAAAAAAAAAIN!!!
**************************************************************************************************/

int main(int argc, char **argv) {
        init(argc, argv, "velocity_dispatcher");

        NodeHandle node;

        if (!setupInputOutput(node))
                return 1;

        spin();
        return 0;
}