dummy_rag_controller_a.cpp

/*! \file dummy_rag_controller_a.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 "ccf/controller/DummyRobotArmController.h"
#include "ccf/connection/NngConnection.h"
#include "ccf/connection/MqttConnection.h"
#include "ccf/util/NodeUtil.h"

#include <google/protobuf/text_format.h>

const std::string CELL_NAME = "place-a";
const std::string NODE_NAME = "rag_connector_dummy_a";

using CetiRosToolbox::getParameter;

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

    GOOGLE_PROTOBUF_VERIFY_VERSION;

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

    ros::NodeHandle n("ceti_connector"); // namespace

    std::string cgv_address = getParameter<std::string>(n, "cgv_address", "tcp://*:6576");

    DummyRobotArmController connector{n, CELL_NAME};

    // only for debugging (uncomment when running without the RAG part)
    // connector.loadScene("/home/jm/work/git/ceti/models2021/src/ccf/config/config_pick_place_scene.json");

    // add an NNG connection
    std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(cgv_address);
    connection->setReceiveTopic(getParameter<std::string>(n, "topics/selection", "selection"));
    connection->setSendTopic(getParameter(n, "topics/scene", CELL_NAME + "/scene/update"));
    connector.addConnection(std::move(connection));

    // add an MQTT connection
    std::string mqtt_address = getParameter<std::string>(n, "mqtt/mqtt_address", "tcp://localhost:1883");
    std::unique_ptr<MqttConnection> mqtt_connection = std::make_unique<MqttConnection>(mqtt_address, NODE_NAME);
    mqtt_connection->addTopic(CELL_NAME + "/scene/init");
    mqtt_connection->addTopic(CELL_NAME + "/command");
    connector.addConnection(std::move(mqtt_connection));

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

    Object *selectedBox{nullptr};
    Object *selectedLocation{nullptr};
    std::optional<std::string> currentlyPickedBox{};
    auto selectionLambda = [&currentlyPickedBox, &connector, &selectedBox, &selectedLocation](
            const Selection &selection) {

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

        Object *object;
        try {
            object = connector.resolveObject(selection.id());
        } catch (std::out_of_range &e) {
            ROS_ERROR_STREAM("[" << NODE_NAME << "] Selected unknown object '" << selection.id() << "'");
            return;
        }
        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::DROP_OFF_LOCATION) {
            selectedLocation = object;
            ROS_INFO_STREAM("[" << NODE_NAME << "] Selected " << type << " '" << selection.id() << "'");
        } else {
            ROS_WARN_STREAM("[" << NODE_NAME << "] Selected unsupported " << type << " '" << selection.id() << "'");
        }

        if (selectedLocation != nullptr && selectedBox != nullptr) {
            auto boxId = selectedBox->id();
            ROS_INFO_STREAM("[" << NODE_NAME << "] Got task: Put " << boxId << " at " << selectedLocation->id());

            // we use the first robot we can find for our picking and placing
            std::optional<Object> robot;
            for (auto &r : connector.getScene()->objects()) {
                if (r.type() == Object::ARM) {
                    robot = r;
                    break;
                }
            }
            if (!robot.has_value()) {
                ROS_ERROR_STREAM("[" << NODE_NAME << "] Unable to find a robot to perform the task.");
                return;
            }

            bool rv = connector.pickAndPlace(*robot, *selectedBox, *selectedLocation, false);
            selectedBox = nullptr;
            selectedLocation = nullptr;
            if (rv) {
                ROS_INFO_STREAM("[" << NODE_NAME << "] Job is done! moved '" << boxId << "'.");
                connector.sendScene();
            } else {
                ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to move box '" << boxId << "'!");
            }
        }
    };
    connector.reactToSelectionMessage(selectionLambda);

    ros::spin();

    return 0;
}