diff --git a/CMakeLists.txt b/CMakeLists.txt index 33af83158ad41cfebdca2157d9f3beba0321f7ff..6a92160483a7ba243d78962214ca7cd8fcc5bbf7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -179,10 +179,10 @@ add_library(scene_collision_object src/scene_collision_object.cpp) ## The recommended prefix ensures that target names across packages don't collide add_executable(${PROJECT_NAME}_dummy_cgv src/dummy_cgv.cpp ${PROTO_SRCS} ${PROTO_HDRS}) add_executable(${PROJECT_NAME}_dummy_pick_place src/dummy_pick_place.cpp ${PROTO_SRCS} ${PROTO_HDRS}) -add_executable(${PROJECT_NAME}_dummy_cgv_connector src/dummy_cgv_connector.cpp ${PROTO_SRCS} ${PROTO_HDRS} src/DummyConnector.cpp src/Connector.cpp src/NngConnection.cpp ) -add_executable(${PROJECT_NAME}_dummy_rag_connector src/dummy_rag_connector.cpp ${PROTO_SRCS} ${PROTO_HDRS} src/DummyConnector.cpp src/Connector.cpp src/NngConnection.cpp src/MqttConnection.cpp) -add_executable(${PROJECT_NAME}_dummy_rbg_connector src/dummy_rbg_connector.cpp ${PROTO_SRCS} ${PROTO_HDRS} src/DummyConnector.cpp src/Connector.cpp src/NngConnection.cpp src/MqttConnection.cpp) -add_executable(${PROJECT_NAME}_moveit_cgv_connector src/moveit_cgv_connector.cpp src/MoveItConnector.cpp src/Connector.cpp ${PROTO_SRCS} ${PROTO_HDRS} src/NngConnection.cpp src/MqttConnection.cpp) +add_executable(${PROJECT_NAME}_dummy_cgv_connector src/dummy_cgv_connector.cpp ${PROTO_SRCS} ${PROTO_HDRS} src/DummyRobotArmController.cpp src/RobotArmController.cpp src/Controller.cpp src/NngConnection.cpp ) +add_executable(${PROJECT_NAME}_dummy_rag_connector src/dummy_rag_connector.cpp ${PROTO_SRCS} ${PROTO_HDRS} src/DummyRobotArmController.cpp src/RobotArmController.cpp src/Controller.cpp src/NngConnection.cpp src/MqttConnection.cpp) +add_executable(${PROJECT_NAME}_dummy_rbg_connector src/dummy_rbg_connector.cpp ${PROTO_SRCS} ${PROTO_HDRS} src/DummyRobotArmController.cpp src/RobotArmController.cpp src/Controller.cpp src/NngConnection.cpp src/MqttConnection.cpp) +add_executable(${PROJECT_NAME}_moveit_cgv_connector src/moveit_cgv_connector.cpp src/MoveItRobotArmController.cpp src/RobotArmController.cpp src/Controller.cpp ${PROTO_SRCS} ${PROTO_HDRS} src/NngConnection.cpp src/MqttConnection.cpp) add_executable(scene_controller src/scene_controller_node.cpp ${PROTO_SRCS} ${PROTO_HDRS}) ## Rename C++ executable without prefix diff --git a/README.md b/README.md index da8d0e02a49f554994d4ebfdf3febead7bbf7f1f..02681fa6784735a0c4bca024a8cddb197cb66987 100644 --- a/README.md +++ b/README.md @@ -28,7 +28,7 @@ for internal and external testing. ``` - You can view the generated files in you browser by opening the generated index.html via CLion in your browser. -## CGV Dummy Connector Demonstrator +## CGV Dummy Controller Demonstrator This demonstrator contains two mock-up nodes that can serve as stand-ins for future functional implementations. diff --git a/include/Controller.h b/include/Controller.h new file mode 100644 index 0000000000000000000000000000000000000000..ddf483a59208103e389977c4058a8b5bf142c031 --- /dev/null +++ b/include/Controller.h @@ -0,0 +1,36 @@ +// +// Created by Johannes Mey on 17/01/2021. +// + +#ifndef CGV_CONNECTOR_WORKSPACE_CONTROLLER_H +#define CGV_CONNECTOR_WORKSPACE_CONTROLLER_H + +#include <functional> +#include <optional> + +#include <ros/ros.h> + +#include "Connection.h" + +#include <connector.pb.h> + +class Controller { + +private: + + +protected: + ros::NodeHandle nodeHandle; + std::vector<std::unique_ptr<Connection>> connections; + virtual void receive(const std::string &channel, const std::string &data) = 0; +public: + explicit Controller(const ros::NodeHandle &nodeHandle); + + // common functionality + void addConnection(std::unique_ptr<Connection> &&connection); + + void sendToAll(const std::string &channel, const std::string &message); + +}; + +#endif //CGV_CONNECTOR_WORKSPACE_CONTROLLER_H diff --git a/include/DummyConnector.h b/include/DummyRobotArmController.h similarity index 76% rename from include/DummyConnector.h rename to include/DummyRobotArmController.h index 56cd5a51d2ab20e4090a1092b6f4773dc3a310c0..7481d60c3d4c974a4ce6a09c269ad3f470927dc1 100644 --- a/include/DummyConnector.h +++ b/include/DummyRobotArmController.h @@ -2,15 +2,15 @@ // Created by Johannes Mey on 17/01/2021. // -#ifndef CGV_CONNECTOR_WORKSPACE_DUMMYCONNECTOR_H -#define CGV_CONNECTOR_WORKSPACE_DUMMYCONNECTOR_H +#ifndef CGV_CONNECTOR_WORKSPACE_DUMMYROBOTARMCONTROLLER_H +#define CGV_CONNECTOR_WORKSPACE_DUMMYROBOTARMCONTROLLER_H -#include "Connector.h" +#include "RobotArmController.h" -class DummyConnector : public Connector { +class DummyRobotArmController : public RobotArmController { public: - explicit DummyConnector(const ros::NodeHandle &nodeHandle, const std::string &cellName); + explicit DummyRobotArmController(const ros::NodeHandle &nodeHandle, const std::string &cellName); bool pickAndDrop(Object &robot, Object &object, Object &bin, bool simulateOnly) override; bool pickAndPlace(Object &robot, Object &object, Object &location, bool simulateOnly) override; @@ -32,4 +32,4 @@ public: virtual bool reachableObject(const Object &robot, const Object &object) override; }; -#endif //CGV_CONNECTOR_WORKSPACE_DUMMYCONNECTOR_H +#endif //CGV_CONNECTOR_WORKSPACE_DUMMYROBOTARMCONTROLLER_H diff --git a/include/MoveItConnector.h b/include/MoveItRobotArmController.h similarity index 64% rename from include/MoveItConnector.h rename to include/MoveItRobotArmController.h index 5de399d5fae1d3d6639695d8927796a870dfc110..9df5f9ac453d35f82a7f5815a2c9e69fa6de0abd 100644 --- a/include/MoveItConnector.h +++ b/include/MoveItRobotArmController.h @@ -2,14 +2,14 @@ // Created by Johannes Mey on 17/01/2021. // -#ifndef CGV_CONNECTOR_WORKSPACE_MOVEITCONNECTOR_H -#define CGV_CONNECTOR_WORKSPACE_MOVEITCONNECTOR_H +#ifndef CGV_CONNECTOR_WORKSPACE_MOVEITROBOTARMCONTROLLER_H +#define CGV_CONNECTOR_WORKSPACE_MOVEITROBOTARMCONTROLLER_H #include <cgv_connector/SceneUpdate.h> -#include "Connector.h" +#include "RobotArmController.h" -class MoveItConnector : public Connector { +class MoveItRobotArmController : public RobotArmController { private: @@ -17,7 +17,7 @@ private: ros::ServiceClient pick_place_client; public: - explicit MoveItConnector(ros::NodeHandle &nodeHandle, const std::string &cellName); + explicit MoveItRobotArmController(ros::NodeHandle &nodeHandle, const std::string &cellName); void updateScene(cgv_connector::SceneUpdateRequest &req, ros::ServiceClient &bin_check_client); @@ -28,4 +28,4 @@ public: }; -#endif //CGV_CONNECTOR_WORKSPACE_MOVEITCONNECTOR_H +#endif //CGV_CONNECTOR_WORKSPACE_MOVEITROBOTARMCONTROLLER_H diff --git a/include/Connector.h b/include/RobotArmController.h similarity index 84% rename from include/Connector.h rename to include/RobotArmController.h index c482125e678a517d3238995000c3f9d87e3729ca..144294608b6853b42341e58b8f2e9b8bad2e92cb 100644 --- a/include/Connector.h +++ b/include/RobotArmController.h @@ -2,8 +2,8 @@ // Created by Johannes Mey on 17/01/2021. // -#ifndef CGV_CONNECTOR_WORKSPACE_CONNECTOR_H -#define CGV_CONNECTOR_WORKSPACE_CONNECTOR_H +#ifndef CGV_CONNECTOR_WORKSPACE_ROBOTARMCONTROLLER_H +#define CGV_CONNECTOR_WORKSPACE_ROBOTARMCONTROLLER_H #include <functional> #include <optional> @@ -11,10 +11,11 @@ #include <ros/ros.h> #include "Connection.h" +#include "Controller.h" #include <connector.pb.h> -class Connector { +class RobotArmController : public Controller { private: @@ -22,7 +23,7 @@ private: std::function<void()> sceneUpdateAction; std::function<void(PickPlace)> pickPlaceAction; - void receive(const std::string &channel, const std::string &data); + void receive(const std::string &channel, const std::string &data) override; bool removeObject(const Object &object); @@ -38,16 +39,12 @@ protected: std::vector<std::unique_ptr<Connection>> connections; std::shared_ptr<Scene> scene; public: - explicit Connector(const ros::NodeHandle &nodeHandle, const std::string &cellName); - // common functionality - void addConnection(std::unique_ptr<Connection> &&connection); + RobotArmController(const ros::NodeHandle &nodeHandle, const std::string &cellName); void loadScene(const std::string& sceneFile); std::shared_ptr<Scene> getScene(); - void sendToAll(const std::string &channel, const std::string &message); - void sendScene(); void initScene(const Scene &scene); @@ -78,4 +75,4 @@ public: Object* resolveObject(const std::string &id); }; -#endif //CGV_CONNECTOR_WORKSPACE_CONNECTOR_H +#endif //CGV_CONNECTOR_WORKSPACE_ROBOTARMCONTROLLER_H diff --git a/src/Controller.cpp b/src/Controller.cpp new file mode 100644 index 0000000000000000000000000000000000000000..bb2d7df483d0c16db1158bfacfb77a113d5f31a4 --- /dev/null +++ b/src/Controller.cpp @@ -0,0 +1,24 @@ +// +// Created by Johannes Mey on 17/01/2021. +// + +#include <fstream> +#include <memory> + +#include "Controller.h" + +void Controller::addConnection(std::unique_ptr<Connection> &&connection) { + connection->initializeConnection([this](auto &&channel, auto &&data) { + receive(std::forward<decltype(channel)>(channel), std::forward<decltype(data)>(data)); + }); + connections.emplace_back(std::move(connection)); +} + +Controller::Controller(const ros::NodeHandle &nodeHandle) : nodeHandle(nodeHandle) { +} + +void Controller::sendToAll(const std::string &channel, const std::string &message) { + for (auto &connection : connections) { + connection->send(channel, message); + } +} diff --git a/src/DummyConnector.cpp b/src/DummyRobotArmController.cpp similarity index 56% rename from src/DummyConnector.cpp rename to src/DummyRobotArmController.cpp index 2c98451102bd27277a9972b8f91db828d99e9cc2..246dfd9dec8c3d17c2f72eef9f24042e883d6a7f 100644 --- a/src/DummyConnector.cpp +++ b/src/DummyRobotArmController.cpp @@ -5,31 +5,31 @@ #include <ros/ros.h> #include <cmath> -#include "DummyConnector.h" -#include "Connector.h" +#include "DummyRobotArmController.h" +#include "Controller.h" -bool DummyConnector::pickAndDrop(Object &robot, Object &object, Object &bin, bool simulateOnly) { +bool DummyRobotArmController::pickAndDrop(Object &robot, Object &object, Object &bin, bool simulateOnly) { ROS_INFO_STREAM("[ros2cgv-dummy] \"Picking and dropping\" for 3 seconds..."); - bool result = Connector::pickAndDrop(robot, object, bin, simulateOnly); + bool result = RobotArmController::pickAndDrop(robot, object, bin, simulateOnly); ros::Rate(ros::Duration(3)).sleep(); sendScene(); return result; } -bool DummyConnector::pickAndPlace(Object &robot, Object &object, Object &location, bool simulateOnly) { +bool DummyRobotArmController::pickAndPlace(Object &robot, Object &object, Object &location, bool simulateOnly) { ROS_INFO_STREAM("[ros2cgv-dummy] \"Picking and placing\" for 3 seconds..."); - bool result = Connector::pickAndPlace(robot, object, location, simulateOnly); + bool result = RobotArmController::pickAndPlace(robot, object, location, simulateOnly); ros::Rate(ros::Duration(3)).sleep(); return result; } -bool DummyConnector::reachableLocation(const Object &robot, const Object &location, const Object &object) { +bool DummyRobotArmController::reachableLocation(const Object &robot, const Object &location, const Object &object) { // we pretend the location is an object and try to reach it return reachableObject(robot, location); } -bool DummyConnector::reachableObject(const Object &robot, const Object &object) { +bool DummyRobotArmController::reachableObject(const Object &robot, const Object &object) { double dx = object.pos().x() - robot.pos().x(); double dy = object.pos().y() - robot.pos().y(); double dz = object.pos().z() - robot.pos().z(); @@ -48,7 +48,7 @@ bool DummyConnector::reachableObject(const Object &robot, const Object &object) return true; } -DummyConnector::DummyConnector(const ros::NodeHandle &nodeHandle, const std::string &cellName) : Connector(nodeHandle, cellName) {} +DummyRobotArmController::DummyRobotArmController(const ros::NodeHandle &nodeHandle, const std::string &cellName) : RobotArmController(nodeHandle, cellName) {} diff --git a/src/MoveItConnector.cpp b/src/MoveItRobotArmController.cpp similarity index 80% rename from src/MoveItConnector.cpp rename to src/MoveItRobotArmController.cpp index 253600fdb659dbd6fb9dbdd3051c673afbabaae1..c3fda6ae6277bd36f64a2fc1c90a3d6f3cee6034 100644 --- a/src/MoveItConnector.cpp +++ b/src/MoveItRobotArmController.cpp @@ -1,4 +1,4 @@ -/*! \file MoveItConnector.cpp +/*! \file MoveItRobotArmController.cpp \author Sebastian Ebert \date 17/01/2021 @@ -15,9 +15,9 @@ #include <cgv_connector/PickPlaceService.h> #include <cgv_connector/BinCheck.h> -#include "MoveItConnector.h" +#include "MoveItRobotArmController.h" -void MoveItConnector::updateScene(cgv_connector::SceneUpdateRequest &req, ros::ServiceClient &bin_check_client) { +void MoveItRobotArmController::updateScene(cgv_connector::SceneUpdateRequest &req, ros::ServiceClient &bin_check_client) { Scene newScene; @@ -88,11 +88,11 @@ void MoveItConnector::updateScene(cgv_connector::SceneUpdateRequest &req, ros::S initScene(newScene); } -bool MoveItConnector::pickAndDrop(Object &robot, Object &object, Object &bin, bool simulateOnly) { +bool MoveItRobotArmController::pickAndDrop(Object &robot, Object &object, Object &bin, bool simulateOnly) { // TODO implement simulateOnly = false if (simulateOnly) { - ROS_ERROR_STREAM("[MoveItConnector] pickAndDrop with simulateOnly=false not implemented yet!"); + ROS_ERROR_STREAM("[MoveItRobotArmController] pickAndDrop with simulateOnly=false not implemented yet!"); } cgv_connector::PickDropService srv; @@ -104,10 +104,10 @@ bool MoveItConnector::pickAndDrop(Object &robot, Object &object, Object &bin, bo return true; } -bool MoveItConnector::pickAndPlace(Object &robot, Object &object, Object &location, bool simulateOnly) { +bool MoveItRobotArmController::pickAndPlace(Object &robot, Object &object, Object &location, bool simulateOnly) { if (simulateOnly) { - ROS_ERROR_STREAM("[MoveItConnector] pickAndPlace with simulateOnly=false not implemented yet!"); + ROS_ERROR_STREAM("[MoveItRobotArmController] pickAndPlace with simulateOnly=false not implemented yet!"); } cgv_connector::PickPlaceService srv; @@ -127,16 +127,16 @@ bool MoveItConnector::pickAndPlace(Object &robot, Object &object, Object &locati } -MoveItConnector::MoveItConnector(ros::NodeHandle &nodeHandle, const std::string &cellName) - : Connector(nodeHandle, cellName), +MoveItRobotArmController::MoveItRobotArmController(ros::NodeHandle &nodeHandle, const std::string &cellName) + : RobotArmController(nodeHandle, cellName), pick_drop_client(nodeHandle.serviceClient<cgv_connector::PickDropService>("/pick_drop_service")), pick_place_client(nodeHandle.serviceClient<cgv_connector::PickPlaceService>("/pick_place_service")) {} -bool MoveItConnector::reachableObject(const Object &robot, const Object &object) { +bool MoveItRobotArmController::reachableObject(const Object &robot, const Object &object) { return false; } -bool MoveItConnector::reachableLocation(const Object &robot, const Object &location, const Object &object) { +bool MoveItRobotArmController::reachableLocation(const Object &robot, const Object &location, const Object &object) { return false; } diff --git a/src/Connector.cpp b/src/RobotArmController.cpp similarity index 79% rename from src/Connector.cpp rename to src/RobotArmController.cpp index af22209d75c589a2c906ef981e6927ca89d3af22..360a23db948bc75b04b102640c0bd6d85fed2399 100644 --- a/src/Connector.cpp +++ b/src/RobotArmController.cpp @@ -9,17 +9,9 @@ #include <ros/package.h> #include <google/protobuf/util/json_util.h> -#include "Connector.h" +#include "RobotArmController.h" -void Connector::addConnection(std::unique_ptr<Connection> &&connection) { - connection->initializeConnection([this](auto &&channel, auto &&data) { - receive(std::forward<decltype(channel)>(channel), std::forward<decltype(data)>(data)); - }); - connections.emplace_back(std::move(connection)); -} - - -void Connector::receive(const std::string &channel, const std::string &data) { +void RobotArmController::receive(const std::string &channel, const std::string &data) { ROS_WARN_STREAM("[ros2cgv] Received message on channel " << channel << "."); if (channel == selectionTopic) { Selection selection; @@ -38,7 +30,7 @@ void Connector::receive(const std::string &channel, const std::string &data) { } } -Object* Connector::resolveObject(const std::string &id) { +Object* RobotArmController::resolveObject(const std::string &id) { ROS_ERROR_STREAM("resolving object with id " << id); for (int i = 0; i<scene->objects_size(); i++) { if (scene->objects(i).id() == id) { @@ -49,7 +41,7 @@ Object* Connector::resolveObject(const std::string &id) { throw std::out_of_range("... did not find it. "); } -void Connector::sendScene() { +void RobotArmController::sendScene() { if (scene) { // meaning if the (smart) pointer is not a nullptr scene->SerializeAsString(); @@ -63,7 +55,7 @@ void Connector::sendScene() { } } -bool Connector::removeObject(const Object &object) { +bool RobotArmController::removeObject(const Object &object) { for (auto it = scene->mutable_objects()->begin(); it != scene->mutable_objects()->end(); ++it) { if (it->id() == object.id()) { @@ -75,11 +67,11 @@ bool Connector::removeObject(const Object &object) { return false; } -bool Connector::pickAndDrop(Object &robot, Object &object, Object &bin, bool simulateOnly) { +bool RobotArmController::pickAndDrop(Object &robot, Object &object, Object &bin, bool simulateOnly) { return removeObject(object); } -void Connector::initScene(const Scene &newScene) { +void RobotArmController::initScene(const Scene &newScene) { if (scene) { // meaning if the (smart) pointer is not a nullptr ROS_INFO_STREAM("[ros2cgv] Resetting scene."); } else { @@ -90,12 +82,12 @@ void Connector::initScene(const Scene &newScene) { sendScene(); } -void Connector::reactToSelectionMessage(std::function<void(Selection)> lambda) { +void RobotArmController::reactToSelectionMessage(std::function<void(Selection)> lambda) { selectionAction = std::move(lambda); } -Connector::Connector(const ros::NodeHandle &nodeHandle, const std::string &cellName) - : nodeHandle(nodeHandle), scene(nullptr), +RobotArmController::RobotArmController(const ros::NodeHandle &nodeHandle, const std::string &cellName) + : Controller(nodeHandle), scene(nullptr), sceneUpdateAction([]() {}), pickPlaceAction([](const PickPlace& p) {}), selectionAction([](const Selection& s) {}), @@ -118,7 +110,7 @@ Connector::Connector(const ros::NodeHandle &nodeHandle, const std::string &cellN } } -bool Connector::pickAndPlace(Object &robot, Object &object, Object &location, bool simulateOnly) { +bool RobotArmController::pickAndPlace(Object &robot, Object &object, Object &location, bool simulateOnly) { if (object.orientation().x() == 0 && object.orientation().y() == 0 && location.orientation().x() == 0 && location.orientation().y() == 0) { // the objects must not be rotated around z // TODO improve float comparison with 0 @@ -134,25 +126,19 @@ bool Connector::pickAndPlace(Object &robot, Object &object, Object &location, bo } -void Connector::reactToPickAndPlaceMessage(std::function<void(PickPlace)> lambda) { +void RobotArmController::reactToPickAndPlaceMessage(std::function<void(PickPlace)> lambda) { pickPlaceAction = std::move(lambda); } -void Connector::reactToSceneUpdateMessage(std::function<void()> lambda) { +void RobotArmController::reactToSceneUpdateMessage(std::function<void()> lambda) { sceneUpdateAction = std::move(lambda); } -std::shared_ptr<Scene> Connector::getScene() { +std::shared_ptr<Scene> RobotArmController::getScene() { return scene; } -void Connector::sendToAll(const std::string &channel, const std::string &message) { - for (auto &connection : connections) { - connection->send(channel, message); - } -} - -void Connector::loadScene(const std::string &sceneFile) { +void RobotArmController::loadScene(const std::string &sceneFile) { // read file into string: https://stackoverflow.com/questions/2602013/read-whole-ascii-file-into-c-stdstring std::ifstream t(sceneFile); diff --git a/src/dummy_cgv_connector.cpp b/src/dummy_cgv_connector.cpp index 692970823eb564ef3ea18621e923cc4ebb0ba42e..e8d5210c2b1d3ba2e239a6f529a6b73a2a159bce 100644 --- a/src/dummy_cgv_connector.cpp +++ b/src/dummy_cgv_connector.cpp @@ -17,7 +17,7 @@ #include "connector.pb.h" -#include "DummyConnector.h" +#include "DummyRobotArmController.h" #include "NngConnection.h" #include <google/protobuf/text_format.h> @@ -25,7 +25,7 @@ const std::string NODE_NAME = "ros2cgv_dummy"; -void loadScene(DummyConnector &connector) { +void loadScene(DummyRobotArmController &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); @@ -74,7 +74,7 @@ int main(int argc, char **argv) { << "/cgv_address from param server, using default " << cgv_address); } - DummyConnector connector{n, "place-a"}; + DummyRobotArmController connector{n, "place-a"}; // add an NNG connection std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(cgv_address); @@ -126,7 +126,7 @@ int main(int argc, char **argv) { 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)) { + if (!connector.RobotArmController::pickAndDrop(*robot, *selectedBox, *selectedBin, false)) { ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to remove box '" << boxId << "'!"); selectedBox = nullptr; selectedBin = nullptr; diff --git a/src/dummy_pick_place.cpp b/src/dummy_pick_place.cpp index b4ada058b880dcf13b14a76386bb4d6de4a33da5..ba63f97343c84a0298ac7a8e5f896ed1d956a70d 100644 --- a/src/dummy_pick_place.cpp +++ b/src/dummy_pick_place.cpp @@ -14,6 +14,7 @@ #include <google/protobuf/text_format.h> #include <random> +#include <optional> #include "connector.pb.h" diff --git a/src/dummy_rag_connector.cpp b/src/dummy_rag_connector.cpp index 0d4d56d30231c9c5868926bb115ca93311df9102..b596a80e97886d871eaf4710a3912bce95044abc 100644 --- a/src/dummy_rag_connector.cpp +++ b/src/dummy_rag_connector.cpp @@ -17,7 +17,7 @@ #include "connector.pb.h" -#include "DummyConnector.h" +#include "DummyRobotArmController.h" #include "NngConnection.h" #include "MqttConnection.h" #include "NodeUtil.h" @@ -39,7 +39,7 @@ int main(int argc, char **argv) { std::string cgv_address = getParameter(n, "cgv_address", "tcp://*:6576"); - DummyConnector connector{n, CELL_NAME}; + DummyRobotArmController connector{n, CELL_NAME}; // only for debugging (uncomment when running without the RAG part) // connector.loadScene("/home/jm/work/git/ceti/models2021/src/cgv_connector/config/config_pick_place_scene.json"); diff --git a/src/dummy_rbg_connector.cpp b/src/dummy_rbg_connector.cpp index b28cf870542a8d7fb23539f8499f656826462d66..145fe6ecd44550495c7783ca07aa968f7a39a909 100644 --- a/src/dummy_rbg_connector.cpp +++ b/src/dummy_rbg_connector.cpp @@ -17,7 +17,7 @@ #include "connector.pb.h" -#include "DummyConnector.h" +#include "DummyRobotArmController.h" #include "NngConnection.h" #include "MqttConnection.h" #include "NodeUtil.h" @@ -39,7 +39,7 @@ int main(int argc, char **argv) { std::string cgv_address = getParameter(n, "cgv_address", "tcp://*:6576"); - DummyConnector connector{n, CELL_NAME}; + DummyRobotArmController connector{n, CELL_NAME}; // only for debugging (uncomment when running without the RAG part) // connector.loadScene("/home/jm/work/git/ceti/models2021/src/cgv_connector/config/config_pick_place_scene.json"); diff --git a/src/moveit_cgv_connector.cpp b/src/moveit_cgv_connector.cpp index 792845747b19f70005af93e0e292f9b4d8a2ffd9..c3de52025ea44026d162fa17d8f82b427826e318 100644 --- a/src/moveit_cgv_connector.cpp +++ b/src/moveit_cgv_connector.cpp @@ -18,7 +18,7 @@ #include <cgv_connector/BinCheck.h> #include <cgv_connector/SceneUpdate.h> -#include "MoveItConnector.h" +#include "MoveItRobotArmController.h" #include "NngConnection.h" const std::string NODE_NAME = "ros2cgv_moveit"; @@ -37,7 +37,7 @@ int main(int argc, char **argv) { << "/cgv_address from param server, using default " << cgv_address); } - MoveItConnector connector{n, "place-a"}; + MoveItRobotArmController connector{n, "place-a"}; // add an NNG connection std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(cgv_address); @@ -87,7 +87,7 @@ int main(int argc, char **argv) { 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)) { + if (!connector.RobotArmController::pickAndDrop(*robot, *selectedBox, *selectedBin, false)) { ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to remove box '" << boxId << "'!"); } else { ROS_INFO_STREAM("[" << NODE_NAME << "] Job is done! '" << boxId << "' is no more.");