diff --git a/launch/dummy-cell_ads-table.launch b/launch/dummy-cell_ads-table.launch index 09cebe099a03dccdafd5af393bbfcc492e76e4b1..d73c4d07317d1746f3e6c5adacb58c69c1e0d88c 100644 --- a/launch/dummy-cell_ads-table.launch +++ b/launch/dummy-cell_ads-table.launch @@ -1,11 +1,9 @@ <launch> <arg name="connection_address" default="tcp://*:6576"/> - <arg name="client_controllers" default="[]"/> - <node pkg="ccf_immersive_sorting" type="dummy_sorting_controller" name="ceti-cell" output="screen"> + <node pkg="ccf_immersive_sorting" type="dummy_sorting_controller" name="ads-cell" 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_ads-table.json"/> </node> diff --git a/launch/dummy-cell_ceti-table.launch b/launch/dummy-cell_ceti-table.launch index 81a4e2039caadea1aeaa437b9fe0de2216a3ef88..a53069b20606ed24873f414ecf622a315afe0dff 100644 --- a/launch/dummy-cell_ceti-table.launch +++ b/launch/dummy-cell_ceti-table.launch @@ -1,11 +1,9 @@ <launch> <arg name="connection_address" default="tcp://*:6576"/> - <arg name="client_controllers" default="[]"/> <node pkg="ccf_immersive_sorting" type="dummy_sorting_controller" name="ceti-cell" 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_ceti-table.json"/> </node> diff --git a/launch/dummy-cell_st-table.launch b/launch/dummy-cell_st-table.launch index 4b52ad02bc6219343d1d87535e0ec78ef956af58..bf815517cecd44190ea6ae66e726fbaa8eb3d1d5 100644 --- a/launch/dummy-cell_st-table.launch +++ b/launch/dummy-cell_st-table.launch @@ -1,11 +1,9 @@ <launch> <arg name="connection_address" default="tcp://*:6576"/> - <arg name="client_controllers" default="[]"/> - <node pkg="ccf_immersive_sorting" type="dummy_sorting_controller" name="ads-cell" output="screen"> + <node pkg="ccf_immersive_sorting" type="dummy_sorting_controller" name="st-cell" 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_st-table.json"/> </node> diff --git a/launch/main_controller_virtual-table.launch b/launch/main_controller_virtual-table.launch index 22b6985aeaec92a71b528bb821c3359e74f3334b..936bc7cb014831cfd0d870e8179dac73869cb24a 100644 --- a/launch/main_controller_virtual-table.launch +++ b/launch/main_controller_virtual-table.launch @@ -1,7 +1,7 @@ <launch> <arg name="connection_address" default="tcp://*:6576"/> - <arg name="client_controllers" default="['ads', 'st', 'ceti']"/> + <arg name="client_controllers" default="['ads-cell', 'st-cell', 'ceti-cell']"/> <node pkg="ccf_immersive_sorting" type="main_controller" name="main_controller" output="screen"> diff --git a/launch/random_selector_automatic.launch b/launch/random_selector_automatic.launch index dfa5a5d5d482f034bea5039a33142eddda9d7031..1561497e774c2d5c87992a766fcc9fb202b531f4 100644 --- a/launch/random_selector_automatic.launch +++ b/launch/random_selector_automatic.launch @@ -1,7 +1,7 @@ <launch> <arg name="autoselect" default="true"/> <arg name="prefix" default="selector"/> - <node pkg="ccf_immersive_sorting" type="dummy_selection_provider" name="my_dummy_selection_provider" + <node pkg="ccf_immersive_sorting" type="dummy_selection_provider" name="automatic_random_selector" output="screen"> <param name="autoselect" type="bool" value="$(arg autoselect)"/> <param name="prefix" type="string" value="$(arg prefix)"/> diff --git a/launch/random_selector_manual.launch b/launch/random_selector_manual.launch index 778ad91e218a5f6aef6a1e345e6cabc7a1577adb..5b38e3eec5c47576e10f2828b4ed042c5229fc8b 100644 --- a/launch/random_selector_manual.launch +++ b/launch/random_selector_manual.launch @@ -1,7 +1,7 @@ <launch> <arg name="autoselect" default="false"/> <arg name="prefix" default="selector"/> - <node pkg="ccf_immersive_sorting" type="dummy_selection_provider" name="my_dummy_selection_provider" + <node pkg="ccf_immersive_sorting" type="dummy_selection_provider" name="manual_random_selector" output="screen"> <param name="autoselect" type="bool" value="$(arg autoselect)"/> <param name="prefix" type="string" value="$(arg prefix)"/> diff --git a/launch/robot-cell_ads-table.launch b/launch/robot-cell_ads-table.launch index c2b1167aaac34130cc304aa11de2005ba7c4b878..5065cc7fba01949fde55fd9eb2cf61c6e873d0b1 100644 --- a/launch/robot-cell_ads-table.launch +++ b/launch/robot-cell_ads-table.launch @@ -2,17 +2,14 @@ <arg name="robot_ip" default="172.31.1.13"/> <arg name="load_gripper" default="true"/> <arg name="connection_address" default="tcp://*:6576"/> - <arg name="client_controllers" default="[]"/> <include file="$(find ccf)/launch/robot_setup.launch"> <arg name="robot_ip" value="$(arg robot_ip)"/> <arg name="load_gripper" value="$(arg load_gripper)"/> </include> - <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="moveit_sorting_controller_instance" - output="screen"> + <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="ads-cell" 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_ads-table.json"/> </node> diff --git a/launch/robot-cell_ceti-table.launch b/launch/robot-cell_ceti-table.launch index edbcecd5e3839093486af52163ae9608d9ccac9b..a2eb31c14b512e4c727912e3f70841b782598a19 100644 --- a/launch/robot-cell_ceti-table.launch +++ b/launch/robot-cell_ceti-table.launch @@ -2,17 +2,14 @@ <arg name="robot_ip" default="172.31.1.13"/> <arg name="load_gripper" default="true"/> <arg name="connection_address" default="tcp://*:6576"/> - <arg name="client_controllers" default="[]"/> <include file="$(find ccf)/launch/robot_setup.launch"> <arg name="robot_ip" value="$(arg robot_ip)"/> <arg name="load_gripper" value="$(arg load_gripper)"/> </include> - <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="moveit_sorting_controller_instance" - output="screen"> + <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="ceti-cell" 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_ceti-table.json"/> </node> diff --git a/launch/robot-cell_st-table.launch b/launch/robot-cell_st-table.launch index 0f58944cc9f93498f8154c9fe55f1bb090c817b8..b6a9e739ff1ff42e481ac92628e227ef4cb4752a 100644 --- a/launch/robot-cell_st-table.launch +++ b/launch/robot-cell_st-table.launch @@ -2,17 +2,14 @@ <arg name="robot_ip" default="172.31.1.13"/> <arg name="load_gripper" default="true"/> <arg name="connection_address" default="tcp://*:6576"/> - <arg name="client_controllers" default="[]"/> <include file="$(find ccf)/launch/robot_setup.launch"> <arg name="robot_ip" value="$(arg robot_ip)"/> <arg name="load_gripper" value="$(arg load_gripper)"/> </include> - <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="moveit_sorting_controller_instance" - output="screen"> + <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="st-cell" 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_st-table.json"/> </node> diff --git a/launch/simulated-cell_ads-table.launch b/launch/simulated-cell_ads-table.launch index 003e30365c1b066dac32c7eccf9f1f92a41cd3af..7e34f0269d0434e165fb04e1faa53099dd9f9e14 100644 --- a/launch/simulated-cell_ads-table.launch +++ b/launch/simulated-cell_ads-table.launch @@ -1,14 +1,11 @@ <launch> <arg name="connection_address" default="tcp://*:6576"/> - <arg name="client_controllers" default="[]"/> <include file="$(find ccf)/launch/simulation_setup.launch"/> - <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="moveit_sorting_controller_instance" - output="screen"> + <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="ads-cell" 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_ads-table.json"/> </node> diff --git a/launch/simulated-cell_ceti-table.launch b/launch/simulated-cell_ceti-table.launch index 1a3dd3cbe9f6a952ba70c2ed170445b2ae47b528..ca6b923c617f94a9bb9a451baea49217605561e8 100644 --- a/launch/simulated-cell_ceti-table.launch +++ b/launch/simulated-cell_ceti-table.launch @@ -1,14 +1,11 @@ <launch> <arg name="connection_address" default="tcp://*:6576"/> - <arg name="client_controllers" default="[]"/> <include file="$(find ccf)/launch/simulation_setup.launch"/> - <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="moveit_sorting_controller_instance" - output="screen"> + <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="ceti-cell" 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_ceti-table.json"/> </node> diff --git a/launch/simulated-cell_st-table.launch b/launch/simulated-cell_st-table.launch index 70f4a2205074985e962ae6627f95ab705cf94431..25563d72ecabefc90b0f86faa9a36cf2878635c2 100644 --- a/launch/simulated-cell_st-table.launch +++ b/launch/simulated-cell_st-table.launch @@ -1,14 +1,11 @@ <launch> <arg name="connection_address" default="tcp://*:6576"/> - <arg name="client_controllers" default="[]"/> <include file="$(find ccf)/launch/simulation_setup.launch"/> - <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="moveit_sorting_controller_instance" - output="screen"> + <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="st-cell" 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_st-table.json"/> </node> diff --git a/src/dummy_sorting_controller.cpp b/src/dummy_sorting_controller.cpp index fcf5a5f1a8e5e7382a38de68d2b14f041d66abc4..52b4147106b230bc0e2d56db3735776436d7f296 100644 --- a/src/dummy_sorting_controller.cpp +++ b/src/dummy_sorting_controller.cpp @@ -42,8 +42,6 @@ int main(int argc, char **argv) { connection->setSendTopic(getParameter(n, "topics/scene", NODE_NAME + "/scene/update")); connector.addConnection(std::move(connection)); - auto clientControllers = getPrivateParameter<std::vector<std::string>>("client_controllers"); - 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")); diff --git a/src/main_controller.cpp b/src/main_controller.cpp index c38d0f8ff759eb4366508711a531d649d3c4836a..37a30dbda5165eba934a433dc1e6617c27575ae7 100644 --- a/src/main_controller.cpp +++ b/src/main_controller.cpp @@ -18,7 +18,7 @@ #include "ccf/connection/MqttConnection.h" #include "ccf/util/NodeUtil.h" -std::string NODE_NAME = "dummy_sorting_controller"; +std::string NODE_NAME = "main_controller"; using CetiRosToolbox::getParameter; using CetiRosToolbox::getPrivateParameter; diff --git a/src/moveit_sorting_controller.cpp b/src/moveit_sorting_controller.cpp index 5da35004149c5846080e1d8dfc80b759e89add23..d6c538d2678b807785ea2b5dc36fe1296adbda9e 100644 --- a/src/moveit_sorting_controller.cpp +++ b/src/moveit_sorting_controller.cpp @@ -42,10 +42,10 @@ int main(int argc, char **argv) { connection->setSendTopic(getParameter(n, "topics/scene", NODE_NAME + "/scene/update")); connector.addConnection(std::move(connection)); - auto clientControllers = getPrivateParameter<std::vector<std::string>>("client_controllers"); - 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)