RobotStateProvider.cpp

//
// Created by sebastian on 31.03.20.
//
#include "ros/ros.h"
#include <gazebo_msgs/LinkStates.h>
#include <mqtt/client.h>
#include "string.h"

// CONFIGURATION START
// ^^^^^^^^^^^^^^^^^^^

// configure the number of not redirected messages between redirected messages
const int message_redirect_rate = 100;

// values to export
bool export_position = true;
bool export_orientation = true;
bool export_twist_linear = true;
bool export_twist_angular = true;
bool export_names = true;

// CONFIGURATION END
// ^^^^^^^^^^^^^^^^^

const std::string SERVER_ADDRESS { "tcp://localhost:1883" };
const std::string CLIENT_ID { "ros_mqtt_client" };
const std::string TOPIC { "robot_data_feed" };
const int QOS = 1;

int current_redirect = 0;
bool isInitialized = false;

/*
 * logs to its own file in /.ros/logs (configured in launch-file)
 */
void exportMessageToLog(const gazebo_msgs::LinkStates & msg)
{
    if(export_names){

        ROS_INFO_STREAM("<<< LINK NAMES >>>");

        for(int i = 0; i < msg.name.size(); i++)
        {
            ROS_INFO_STREAM("[" << i << "] " << msg.name.at(i));
        }
    }

    if(export_position){

        ROS_INFO_STREAM("<<< POSITIONS BY LINK >>>");

        for(int i = 0; i < msg.pose.size(); i++)
        {
            ROS_INFO_STREAM("[" << i << "] [x] " << msg.pose.at(i).position.x);
            ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).position.y);
            ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).position.z);
        }
    }

    if(export_orientation){

        ROS_INFO_STREAM("<<< ORIENTATIONS BY LINK >>>");

        for(int i = 0; i < msg.pose.size(); i++)
        {
            ROS_INFO_STREAM("[" << i << "] [w] " <<  msg.pose.at(i).orientation.w);
            ROS_INFO_STREAM("[" << i << "] [x] " <<  msg.pose.at(i).orientation.x);
            ROS_INFO_STREAM("[" << i << "] [y] " <<  msg.pose.at(i).orientation.y);
            ROS_INFO_STREAM("[" << i << "] [z] " <<  msg.pose.at(i).orientation.z);
        }
    }

    if(export_twist_linear){
        ROS_INFO_STREAM("<<< LINEAR TWISTS BY LINK >>>");
        for(int i = 0; i < msg.twist.size(); i++)
        {
            ROS_INFO_STREAM("[" << i << "] [x] " <<  msg.twist.at(i).linear.x);
            ROS_INFO_STREAM("[" << i << "] [y] " <<  msg.twist.at(i).linear.y);
            ROS_INFO_STREAM("[" << i << "] [z] " <<  msg.twist.at(i).linear.z);
        }
    }

    if(export_twist_angular){
        ROS_INFO_STREAM("<<< ANGULAR TWISTS BY LINK >>>");
        for(int i = 0; i < msg.twist.size(); i++)
        {
            ROS_INFO_STREAM("[" << i << "] [x] " <<  msg.twist.at(i).angular.x);
            ROS_INFO_STREAM("[" << i << "] [y] " <<  msg.twist.at(i).angular.y);
            ROS_INFO_STREAM("[" << i << "] [z] " <<  msg.twist.at(i).angular.z);
        }
    }
}

void providerCallback(const gazebo_msgs::LinkStates & msg)
{
    if(current_redirect != 0 && current_redirect != message_redirect_rate){
        //std::cout << "Ignoring message (redirect: " << current_redirect << " )" << std::endl;
        current_redirect++;
        return;
    }

    if(current_redirect == 0 || current_redirect == message_redirect_rate){
        //std::cout << "Redirecting message:" << msg.pose.at(1).position << std::endl;
        exportMessageToLog(msg);
        if(current_redirect == message_redirect_rate){
            current_redirect = 1;
        } else{
            current_redirect++;
        }
    }
}

void sendMqttMessage(){
    mqtt::client client(SERVER_ADDRESS, CLIENT_ID);

    mqtt::connect_options connOpts;
    connOpts.set_keep_alive_interval(20);
    connOpts.set_clean_session(true);

    client.connect(connOpts);
    auto pubmsg = mqtt::make_message(TOPIC, "test");
    pubmsg->set_qos(QOS);
    client.publish(pubmsg);

    client.disconnect();
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "listener");
    ros::NodeHandle n;
    n.getParam("ready", isInitialized);

    // make sure the robot is initialized
    while(!isInitialized){
        //std::cout << "Waiting" << std::endl;
        n.getParam("tud_planner_ready", isInitialized);
    }

    //sendMqttMessage();

    ros::Subscriber sub = n.subscribe("/gazebo/link_states", 10000, providerCallback);
    ros::spin();

    return 0;
}