diff --git a/CMakeLists.txt b/CMakeLists.txt index 71db6cf8e4d639fa081e9499a09af86cd6977592..c2491a880ae96e0f4dcca33cd51d6f9e386037bd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -136,7 +136,7 @@ include_directories( ## The recommended prefix ensures that target names across packages don't collide add_executable(${PROJECT_NAME}_dummy_selection_provider src/dummy_selection_provider.cpp ${PROTO_SRCS} ${PROTO_HDRS}) add_executable(${PROJECT_NAME}_dummy_sorting_controller src/dummy_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS} ) -add_executable(${PROJECT_NAME}_dummy_main_controller src/dummy_main_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS} ) +add_executable(${PROJECT_NAME}_main_controller src/main_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS} ) add_executable(${PROJECT_NAME}_moveit_sorting_controller src/moveit_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS}) @@ -146,21 +146,21 @@ add_executable(${PROJECT_NAME}_moveit_sorting_controller src/moveit_sorting_cont ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" set_target_properties(${PROJECT_NAME}_dummy_selection_provider PROPERTIES OUTPUT_NAME dummy_selection_provider PREFIX "") set_target_properties(${PROJECT_NAME}_dummy_sorting_controller PROPERTIES OUTPUT_NAME dummy_sorting_controller PREFIX "") -set_target_properties(${PROJECT_NAME}_dummy_main_controller PROPERTIES OUTPUT_NAME dummy_main_controller PREFIX "") +set_target_properties(${PROJECT_NAME}_main_controller PROPERTIES OUTPUT_NAME main_controller PREFIX "") set_target_properties(${PROJECT_NAME}_moveit_sorting_controller PROPERTIES OUTPUT_NAME moveit_sorting_controller PREFIX "") ## Add cmake target dependencies of the executable ## same as for the library above add_dependencies(${PROJECT_NAME}_dummy_selection_provider ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) add_dependencies(${PROJECT_NAME}_dummy_sorting_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -add_dependencies(${PROJECT_NAME}_dummy_main_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(${PROJECT_NAME}_main_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) add_dependencies(${PROJECT_NAME}_moveit_sorting_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against target_link_libraries(${PROJECT_NAME}_dummy_sorting_controller ${catkin_LIBRARIES} ) -target_link_libraries(${PROJECT_NAME}_dummy_main_controller +target_link_libraries(${PROJECT_NAME}_main_controller ${catkin_LIBRARIES} ) target_link_libraries(${PROJECT_NAME}_moveit_sorting_controller diff --git a/launch/virtual-table_dummy_main.launch b/launch/virtual-table_main_controller.launch similarity index 69% rename from launch/virtual-table_dummy_main.launch rename to launch/virtual-table_main_controller.launch index c8ab675828c25f539c2871182b620b0cc45c23c9..f8637648645aa03f0e3494c0f2f1243a3411457c 100644 --- a/launch/virtual-table_dummy_main.launch +++ b/launch/virtual-table_main_controller.launch @@ -1,10 +1,10 @@ <launch> <arg name="connection_address" default="tcp://*:6576" /> - <arg name="client_controllers" default="[]" /> + <arg name="client_controllers" default="['ads', 'st', 'ceti']" /> - <node pkg="ccf_immersive_sorting" type="dummy_main_controller" name="dummy_main_controller_instance" output="screen" > + <node pkg="ccf_immersive_sorting" type="main_controller" name="main_controller" output="screen" > <param name="connection_address" type="string" value="$(arg connection_address)" /> <param name="client_controllers" type="yaml" value="$(arg client_controllers)" /> <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene_virtual-table.json" /> diff --git a/src/dummy_main_controller.cpp b/src/main_controller.cpp similarity index 85% rename from src/dummy_main_controller.cpp rename to src/main_controller.cpp index ecfea69d0610fa7698c6c5ba15709f7fbfe101f3..ba9a07684513bbdcc127742f50451448b265b386 100644 --- a/src/dummy_main_controller.cpp +++ b/src/main_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"; @@ -47,6 +48,9 @@ int main(int argc, char **argv) { DummyRobotArmController connector{n, NODE_NAME}; + std::map<std::string, Scene> clients; + std::vector<std::pair<std::string, std::string>> actions; + // add an NNG connection std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(connectionAddress); connection->setReceiveTopic(getParameter<std::string>(n, "topics/selection", "selection")); @@ -54,14 +58,16 @@ int main(int argc, char **argv) { connector.addConnection(std::move(connection)); auto clientControllers = getPrivateParameter<std::vector<std::string>>("client_controllers"); + auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883"); ROS_INFO_STREAM("Connecting to " << clientControllers.size() << " client controllers."); + std::shared_ptr<MqttConnection> client_connection = std::make_shared<MqttConnection>(NODE_NAME, mqttServer); + connector.addConnection(client_connection); 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 scene_update_topic = "/" + client + getParameter(n, "topics/scene", NODE_NAME + "/scene/update"); + client_connection->listen(scene_update_topic); + connector.addCallback(scene_update_topic, [client, &clients](auto msg) { clients[client].ParseFromString(msg);}); } connector.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") + @@ -77,6 +83,16 @@ int main(int argc, char **argv) { ros::Timer timer = n.createTimer(ros::Duration(10), [&connector]( const ros::TimerEvent &event) { connector.sendScene(); }); // send a scene every ten seconds + + ros::Timer timer_scene_log = n.createTimer(ros::Duration(10), [&clients]( + const ros::TimerEvent &event) { + + for (const auto &[name, scene] : clients) { + ROS_WARN_STREAM("CLIENT " << name << " has a scene with " << scene.objects_size() << "objects"); + } + + }); + auto selectionMessageCallback = [¤tlyPickedBox, &connector, &robot, &selectedBox, &selectedBin, &n]( const Selection &selection) {