dummy_cgv_connector.cpp

/*! \file dummy_cgv_connector.cpp

    A ROS node that simulates a node connected to a model-based cobotic application

    \author Johannes Mey
    \author Sebastian Ebert
    \date 07.07.20
*/

#define BOOST_BIND_GLOBAL_PLACEHOLDERS // fix boost

#include <fstream>

#include <ros/ros.h>
#include <ros/package.h>
#include <std_msgs/Empty.h>

#include "connector.pb.h"

#include "DummyConnector.h"
#include "NngConnection.h"

#include <google/protobuf/text_format.h>
#include <google/protobuf/util/json_util.h>

const std::string NODE_NAME = "ros2cgv_dummy";

void loadScene(DummyConnector &connector) {
    // read file into string: https://stackoverflow.com/questions/2602013/read-whole-ascii-file-into-c-stdstring
    std::string path = ros::package::getPath("cgv_connector") + "/config/config_scene.yaml";
    std::ifstream t(path);

    if (!t.is_open()) {
        ROS_ERROR_STREAM("[" << NODE_NAME << "] Unable to open scene config file " << path);
    }

    std::string str;

    t.seekg(0, std::ios::end); // skip to end of file
    str.reserve(t.tellg()); // reserve memory in the string of the size of the file
    t.seekg(0, std::ios::beg); // go back to the beginning

    // Note the double parentheses! F**k C++! https://en.wikipedia.org/wiki/Most_vexing_parse
    str.assign((std::istreambuf_iterator<char>(t)), std::istreambuf_iterator<char>());

    Scene newScene;
    google::protobuf::util::Status status = google::protobuf::util::JsonStringToMessage(str, &newScene);
    if (!status.ok()) {
        ROS_ERROR_STREAM("[" << NODE_NAME << "] Unable to parse Json String: " << status.ToString());
    } else {
        ROS_INFO_STREAM("[" << NODE_NAME << "] Parsed a scene with " << newScene.objects().size() << " objects.");
        connector.initScene(newScene);
    }

    std::string s;
    if (google::protobuf::TextFormat::PrintToString(newScene, &s)) {
        ROS_DEBUG_STREAM("[" << NODE_NAME << "] Received scene" << std::endl << s);
    } else {
        ROS_WARN_STREAM("[" << NODE_NAME << "] Scene invalid! partial content: " << newScene.ShortDebugString());
    }
}

int main(int argc, char **argv) {

    GOOGLE_PROTOBUF_VERIFY_VERSION;

    ros::init(argc, argv, NODE_NAME);

    ros::NodeHandle n("connector_node_ros_cgv"); // namespace where the cgv_address is

    std::string cgv_address = "tcp://*:6576";
    if (!n.getParam("cgv_address", cgv_address)) {
        ROS_WARN_STREAM("[" << NODE_NAME << "] Could not get string value for " << n.getNamespace()
                            << "/cgv_address from param server, using default " << cgv_address);
    }

    DummyConnector connector{n, "place-a"};

    // add an NNG connection
    std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(cgv_address);
    std::string selectionTopic = "selection";
    if (!n.getParam("topics/selection", selectionTopic)) {
        ROS_WARN_STREAM("[" << NODE_NAME << "] Could not get string value for " << n.getNamespace()
                            << "/topics/selection from param server, using default " << selectionTopic);
    }
    connection->setReceiveTopic(selectionTopic);
    std::string sceneTopic = "scene";
    if (!n.getParam("topics/scene", sceneTopic)) {
        ROS_WARN_STREAM("[" << NODE_NAME << "] Could not get string value for " << n.getNamespace()
                            << "/topics/scene from param server, using default " << sceneTopic);
    }
    connection->setSendTopic(sceneTopic);
    connector.addConnection(std::move(connection));

    loadScene(connector);

    Object* robot = nullptr;
    Object* selectedBox = nullptr;
    Object* selectedBin = nullptr;
    std::optional<std::string> currentlyPickedBox{};

    ros::Subscriber sub = n.subscribe<std_msgs::Empty>("send_scene", 1000, [&connector](
            const std_msgs::EmptyConstPtr &msg) { connector.sendScene(); });
    ros::Timer timer = n.createTimer(ros::Duration(10), [&connector](
            const ros::TimerEvent &event) { connector.sendScene(); }); // send a scene every ten seconds

    auto lambda = [&currentlyPickedBox, &connector, &robot, &selectedBox, &selectedBin](const Selection &selection) {

        if (currentlyPickedBox.has_value()) {
            ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to accept selections while picking is in progress.");
            return;
        }

        auto object = connector.resolveObject(selection.id());
        auto type = Object::Type_Name(object->type());
        if (object->type() == Object::BOX) {
            selectedBox = object;
            ROS_INFO_STREAM("[" << NODE_NAME << "] Selected " << type << " '" << selection.id() << "'");
        } else if (object->type() == Object::BIN) {
            selectedBin = object;
            ROS_INFO_STREAM("[" << NODE_NAME << "] Selected " << type << " '" << selection.id() << "'");
        } else {
            ROS_WARN_STREAM("[" << NODE_NAME << "] Selected unsupported " << type << " '" << selection.id() << "'");
        }

        if (selectedBin && selectedBox) {
            auto boxId = selectedBox->id();
            ROS_INFO_STREAM("[" << NODE_NAME << "] Got task: Put " << boxId << " into " << selectedBin->id());
            if (!connector.Connector::pickAndDrop(*robot, *selectedBox, *selectedBin, false)) {
                ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to remove box '" << boxId << "'!");
                selectedBox = nullptr;
                selectedBin = nullptr;
            } else {
                ROS_INFO_STREAM("[" << NODE_NAME << "] Job is done! '" << boxId << "' is no more.");
                selectedBox = nullptr;
                selectedBin = nullptr;
                connector.sendScene();
            }
        }
    };

    connector.reactToSelectionMessage(lambda);

    ros::spin();

    return 0;
}