moveit_cgv_controller.cpp

/*! \file moveit_cgv_controller.cpp

    A ROS node that controls a robot 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 <ros/ros.h>
#include <std_msgs/Empty.h>

#include "connector.pb.h"

#include "ccf/controller/MoveItRobotArmController.h"
#include "ccf/connection/NngConnection.h"
#include "ccf/util/NodeUtil.h"

const std::string NODE_NAME = "ros2cgv_moveit";

using CetiRosToolbox::getParameter;

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);
    }

    MoveItRobotArmController connector{n, NODE_NAME};

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

    // scene loading is not required, the scene is updated by moveit

    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 selectionMessageCallback = [&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;
        }

        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::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());
            currentlyPickedBox = boxId;
            if (!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(selectionMessageCallback);

    auto sceneUpdateCallback = [&currentlyPickedBox, &connector]() {
        if (currentlyPickedBox.has_value()) {
            try {
                connector.resolveObject(currentlyPickedBox.value());
            } catch (std::out_of_range &e) {
                ROS_INFO_STREAM(
                        "[" << NODE_NAME << "] box " << currentlyPickedBox.value() << " has been removed from the scene!");
                currentlyPickedBox.reset();
            }
        }
    };
    connector.reactToSceneUpdateMessage(sceneUpdateCallback);

    ros::spin();

    return 0;
}