diff --git a/src/dummy_sorting_controller.cpp b/src/dummy_sorting_controller.cpp index d1580aebb81fadc74332dc825eb70909f21f6a86..0e06b1456a7a97ac0806bfb8dc052a11c36b2bbb 100644 --- a/src/dummy_sorting_controller.cpp +++ b/src/dummy_sorting_controller.cpp @@ -15,6 +15,7 @@ #include "ccf/controller/DummyRobotArmController.h" #include "ccf/connection/NngConnection.h" +#include "ccf/connection/MqttConnection.h" #include "ccf/util/NodeUtil.h" std::string NODE_NAME = "dummy_sorting_controller"; @@ -43,14 +44,12 @@ int main(int argc, char **argv) { auto clientControllers = getPrivateParameter<std::vector<std::string>>("client_controllers"); - ROS_INFO_STREAM("Connecting to " << clientControllers.size() << " client controllers."); - for (const auto &client: clientControllers) { - ROS_INFO_STREAM("Connecting to client at " << client << "."); - std::unique_ptr<NngConnection> client_connection = std::make_unique<NngConnection>(client, false); - client_connection->setSendTopic(getParameter<std::string>(n, "topics/selection", "selection")); - client_connection->setReceiveTopic("client_scene"); - connector.addConnection(std::move(client_connection)); - } + auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883"); + std::unique_ptr<MqttConnection> mqtt_connection = std::make_unique<MqttConnection>(mqttServer, ros::this_node::getName()); + mqtt_connection->listen(getParameter<std::string>(n, "topics/selection", "selection")); + connector.addConnection(std::move(mqtt_connection)); + + ros::WallDuration(5).sleep(); // wait 5 secs to init scene (to give moveit time to load) connector.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") + "/config/config_scene.yaml")); diff --git a/src/moveit_sorting_controller.cpp b/src/moveit_sorting_controller.cpp index 8e5d94886d5f4a13c21626de56d93bf7b9a6b403..28863b448bdef9781f4594e4b5cdfb3ad845eb72 100644 --- a/src/moveit_sorting_controller.cpp +++ b/src/moveit_sorting_controller.cpp @@ -15,6 +15,7 @@ #include "ccf/controller/MoveItRobotArmController.h" #include "ccf/connection/NngConnection.h" +#include "ccf/connection/MqttConnection.h" #include "ccf/util/NodeUtil.h" std::string NODE_NAME = "moveit_sorting_controller"; @@ -43,14 +44,8 @@ int main(int argc, char **argv) { auto clientControllers = getPrivateParameter<std::vector<std::string>>("client_controllers"); - ROS_INFO_STREAM("Connecting to " << clientControllers.size() << " client controllers."); - for (const auto &client: clientControllers) { - ROS_INFO_STREAM("Connecting to client at " << client << "."); - std::unique_ptr<NngConnection> client_connection = std::make_unique<NngConnection>(client, false); - client_connection->setSendTopic(getParameter<std::string>(n, "topics/selection", "selection")); - client_connection->setReceiveTopic("client_scene"); - connector.addConnection(std::move(client_connection)); - } + auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883"); + std::unique_ptr<MqttConnection> mqtt_connection = std::make_unique<MqttConnection>(mqttServer, ros::this_node::getName()); ros::WallDuration(5).sleep(); // wait 5 secs to init scene (to give moveit time to load)