dummy_rbg_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 "MqttConnection.h"
#include "NodeUtil.h"

#include <google/protobuf/text_format.h>

const std::string CELL_NAME = "place-b";
const std::string NODE_NAME = "rag_connector_dummy_b";

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(n, "cgv_address", "tcp://*:6576");

    DummyConnector connector{n, CELL_NAME};

    // add an MQTT connection
    std::string mqtt_address = getParameter(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

    auto pickPlaceLambda = [&connector](const PickPlace &command) {
        ROS_INFO_STREAM("[" << NODE_NAME << "] Received a command to pick object " << command.idpick()
                            << " and place it at " << command.idplace()
                            << " using robot " << command.idrobot() << ".");

        auto robot = connector.resolveObject(command.idrobot());
        auto object = connector.resolveObject(command.idpick());
        auto location = connector.resolveObject(command.idplace());

        // check the parts of the command
        if (!robot) {
            ROS_ERROR_STREAM("Unknown robot " << command.idrobot() << ". Aborting PickPlace command.");
            return;
        } else if (robot->type() != Object::ARM) {
            ROS_ERROR_STREAM("Robot " << command.idrobot() << " has wrong type " << robot->type()
                                      << ". Aborting PickPlace command.");
        }

        if (!object) {
            ROS_ERROR_STREAM("Unknown object " << command.idpick() << ". Aborting PickPlace command.");
            return;
        } else if (object->type() != Object::BOX) {
            ROS_ERROR_STREAM("Object " << command.idpick() << " has wrong type " << object->type()
                                       << ". Aborting PickPlace command.");
        }

        if (!location) {
            ROS_ERROR_STREAM("Unknown place location " << command.idplace() << ". Aborting PickPlace command.");
            return;
        } else if (location->type() != Object::DROP_OFF_LOCATION) {
            ROS_ERROR_STREAM("Location " << command.idplace() << " has wrong type " << location->type()
                                         << ". Aborting PickPlace command.");
        }

        // by now, we know that the request is legit, so we call the pick and place action
        if (connector.pickAndPlace(*robot, *object, *location, false)) {
            ROS_INFO_STREAM("[" << NODE_NAME << "] Command successful.");
        } else {
            ROS_INFO_STREAM("[" << NODE_NAME << "] Command failed.");
        }

    };
    connector.reactToPickAndPlaceMessage(pickPlaceLambda);

    std::string reachabilityTopicPrefix = CELL_NAME + "/reachability/";

    auto sceneUpdateLambda = [&reachabilityTopicPrefix, &connector]() {
        ROS_INFO_STREAM("[" << NODE_NAME << "] Scene was updated, sending new reachability information.");

        for (auto &robot : connector.getScene()->objects()) {
            if (robot.type() == Object::ARM) {
                Reachability reachability;
                reachability.set_idrobot(robot.id());
                for (auto &object : connector.getScene()->objects()) {
                    if (object.type() == Object::DROP_OFF_LOCATION) {
                        auto r = reachability.add_objects();
                        r->set_idobject(object.id());
                        // FIXME the second object is wrong here, but ignored in the implementation
                        r->set_reachable(connector.reachableLocation(robot, object, object));
                    } else if (object.type() == Object::BOX) {
                        auto r = reachability.add_objects();
                        r->set_idobject(object.id());
                        r->set_reachable(connector.reachableObject(robot, object));
                    }
                    // otherwise skip the object
                }
                connector.sendToAll(reachabilityTopicPrefix + robot.id(), reachability.SerializeAsString());
            }
        }

    };
    connector.reactToSceneUpdateMessage(sceneUpdateLambda);

    ros::spin();

    return 0;
}