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"
#include "linkstate.pb.h"
#include "mem_persistence.cpp"

// 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_to_log = false;

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

/*
 * mqtt-topics for all links
 * ground, link0, link1, link2, link3, link4, link5, link6, link7 (end-effector), left finger, right finder
 */
std::string topics [11] = {"panda_link_0","panda_link_1","panda_link_2" ,"panda_link_3",
                           "panda_link_4","panda_link_5","panda_link_6","panda_link_7",
                           "panda_link_8","panda_link_9","panda_link_10"};

bool mqttSetup = false;
bool isInitialized = false;

const int QOS = 1;
int current_redirect = 0;

mem_persistence persist;
mqtt::client client("tcp://localhost:1883", "robot_state_provider_mqtt", &persist);

/*
 * logs to its own file in /.ros/logs (configured in launch-file)
 */
void exportMessageToLog(const gazebo_msgs::LinkStates &msg) {
    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);
        }
    }
}

panda::PandaLinkState buildLinkStateMessage(const gazebo_msgs::LinkStates &msg, int link_number) {

    panda::PandaLinkState pls_msg;

    pls_msg.set_name(msg.name.at(link_number));

    if (export_position) {
        panda::PandaLinkState_Position *pos = new panda::PandaLinkState_Position();
        pos->set_positionx(msg.pose.at(link_number).position.x);
        pos->set_positiony(msg.pose.at(link_number).position.y);
        pos->set_positionz(msg.pose.at(link_number).position.z);
        pls_msg.set_allocated_pos(pos);
    }

    if (export_orientation) {
        panda::PandaLinkState_Orientation *orient = new panda::PandaLinkState_Orientation();
        orient->set_orientationw(msg.pose.at(link_number).orientation.w);
        orient->set_orientationx(msg.pose.at(link_number).orientation.x);
        orient->set_orientationy(msg.pose.at(link_number).orientation.y);
        orient->set_orientationz(msg.pose.at(link_number).orientation.z);
        pls_msg.set_allocated_orient(orient);
    }

    if (export_twist_linear) {
        panda::PandaLinkState_TwistLinear *tl = new panda::PandaLinkState_TwistLinear();
        tl->set_twistlinearx(msg.twist.at(link_number).linear.x);
        tl->set_twistlineary(msg.twist.at(link_number).linear.y);
        tl->set_twistlinearz(msg.twist.at(link_number).linear.z);
        pls_msg.set_allocated_tl(tl);
    }

    if (export_twist_angular) {
        panda::PandaLinkState_TwistAngular *ta = new panda::PandaLinkState_TwistAngular();
        ta->set_twistangularx(msg.twist.at(link_number).angular.x);
        ta->set_twistangulary(msg.twist.at(link_number).angular.y);
        ta->set_twistangularz(msg.twist.at(link_number).angular.z);
        pls_msg.set_allocated_ta(ta);
    }

    return pls_msg;
}

void setupMqtt() {

    if (!mqttSetup) {
        std::cout << "Initializing..." << std::endl;

        mqtt::connect_options connOpts;
        connOpts.set_keep_alive_interval(20);
        connOpts.set_clean_session(true);
        std::cout << "...OK" << std::endl;

        std::cout << "\nConnecting..." << std::endl;
        client.connect(connOpts);
        std::cout << "...OK" << std::endl;
        mqttSetup = true;
    }
}


void sendMqttMessages(const gazebo_msgs::LinkStates &msg) {

    setupMqtt();

    for(int i = 0; i < msg.name.size(); i++)
    {
        panda::PandaLinkState pls_msg = buildLinkStateMessage(msg, i);
        std::string mqtt_msg;
        pls_msg.SerializeToString(&mqtt_msg);

        //std::cout << "SENDING MESSAGE" << std::endl;
        auto pubmsg = mqtt::make_message(topics[i], mqtt_msg);
        pubmsg->set_qos(QOS);
        client.publish(pubmsg);
    }
}

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;

        if(export_to_log){
            exportMessageToLog(msg);
        }

        sendMqttMessages(msg);

        if (current_redirect == message_redirect_rate) {
            current_redirect = 1;
        } else {
            current_redirect++;
        }
    }
}

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

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

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

    // disconnect from mqtt
    client.disconnect();
    return 0;
}