From 064b11e32475d1b8c5d51d4a3010c09e0fbe0707 Mon Sep 17 00:00:00 2001 From: Johannes Mey <johannes.mey@tu-dresden.de> Date: Tue, 31 Aug 2021 10:50:22 +0200 Subject: [PATCH] add dependent controllers that get the forwarded selection of a main controller --- config/config.yaml | 2 -- ...ble__dummy_controller_and_dummy_cgv.launch | 11 ++++++++++ .../cleaning/cleaning_dummy_controller.launch | 10 ++++++++- ...ning_dummy_controller_and_dummy_cgv.launch | 2 +- .../cleaning/cleaning_robot_controller.launch | 7 +++++- ...ning_robot_controller_and_dummy_cgv.launch | 14 ++---------- launch/cleaning/cleaning_simulation.launch | 8 ++++++- .../cleaning_simulation_and_dummy_cgv.launch | 10 +++------ src/ccf/controller/RobotArmController.cpp | 2 -- src/dummy_cgv_controller.cpp | 22 ++++++++++++++++--- src/dummy_rag_controller_a.cpp | 3 ++- src/dummy_rag_controller_b.cpp | 3 ++- src/moveit_cgv_controller.cpp | 22 ++++++++++++++++--- 13 files changed, 81 insertions(+), 35 deletions(-) create mode 100644 launch/cleaning/cleaning_double__dummy_controller_and_dummy_cgv.launch diff --git a/config/config.yaml b/config/config.yaml index 97af7cc..22d7c81 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -1,5 +1,3 @@ -connector_node_ros_cgv: - cgv_address: "tcp://*:6576" max_grasp_approach_velocity: 0.2 max_grasp_approach_acceleration: 0.2 max_grasp_transition_velocity: 0.2 diff --git a/launch/cleaning/cleaning_double__dummy_controller_and_dummy_cgv.launch b/launch/cleaning/cleaning_double__dummy_controller_and_dummy_cgv.launch new file mode 100644 index 0000000..757a39d --- /dev/null +++ b/launch/cleaning/cleaning_double__dummy_controller_and_dummy_cgv.launch @@ -0,0 +1,11 @@ +<launch> + <node pkg="ccf" type="dummy_cgv" name="my_dummy_cgv" output="screen" /> + <node pkg="ccf" type="dummy_cgv_controller" name="my_first_dummy_cgv_controller" output="screen"> + <param name="connection_address" type="string" value="tcp://*:6576" /> + <param name="client_controllers" type="yaml" value="['tcp://127.0.0.1:6586']" /> + </node> + <node pkg="ccf" type="dummy_cgv_controller" name="my_second_dummy_cgv_controller" output="screen"> + <param name="connection_address" type="string" value="tcp://*:6586" /> + <param name="client_controllers" type="yaml" value="[]" /> + </node> +</launch> diff --git a/launch/cleaning/cleaning_dummy_controller.launch b/launch/cleaning/cleaning_dummy_controller.launch index 3b28246..7ce2c9c 100644 --- a/launch/cleaning/cleaning_dummy_controller.launch +++ b/launch/cleaning/cleaning_dummy_controller.launch @@ -1,3 +1,11 @@ <launch> - <node pkg="ccf" type="dummy_cgv_controller" name="my_dummy_cgv_controller" output="screen" /> + + <arg name="connection_address" default="tcp://*:6576" /> + <arg name="client_controllers" default="[]" /> + + <node pkg="ccf" type="dummy_cgv_controller" name="my_dummy_cgv_controller" output="screen"> + <param name="connection_address" type="string" value="$(arg connection_address)" /> + <param name="client_controllers" type="yaml" value="$(arg client_controllers)" /> + </node> + </launch> diff --git a/launch/cleaning/cleaning_dummy_controller_and_dummy_cgv.launch b/launch/cleaning/cleaning_dummy_controller_and_dummy_cgv.launch index ebb269c..0caf484 100644 --- a/launch/cleaning/cleaning_dummy_controller_and_dummy_cgv.launch +++ b/launch/cleaning/cleaning_dummy_controller_and_dummy_cgv.launch @@ -1,4 +1,4 @@ <launch> <node pkg="ccf" type="dummy_cgv" name="my_dummy_cgv" output="screen" /> - <node pkg="ccf" type="dummy_cgv_controller" name="my_dummy_cgv_controller" output="screen" /> + <include file="$(find ccf)/launch/cleaning/cleaning_dummy_controller.launch" /> </launch> diff --git a/launch/cleaning/cleaning_robot_controller.launch b/launch/cleaning/cleaning_robot_controller.launch index a44470f..206780e 100644 --- a/launch/cleaning/cleaning_robot_controller.launch +++ b/launch/cleaning/cleaning_robot_controller.launch @@ -2,12 +2,17 @@ <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/cleaning/_robot_setup.launch" > <arg name="robot_ip" value="$(arg robot_ip)" /> <arg name="load_gripper" value="$(arg load_gripper)"/> </include> - <node pkg="ccf" type="moveit_cgv_controller" name="moveit_cgv_controller_instance" output="screen" /> + <node pkg="ccf" type="moveit_cgv_controller" name="moveit_cgv_controller_instance" output="screen" > + <param name="connection_address" type="string" value="$(arg connection_address)" /> + <param name="client_controllers" type="yaml" value="$(arg client_controllers)" /> + </node> </launch> \ No newline at end of file diff --git a/launch/cleaning/cleaning_robot_controller_and_dummy_cgv.launch b/launch/cleaning/cleaning_robot_controller_and_dummy_cgv.launch index d5e84b2..190cb85 100644 --- a/launch/cleaning/cleaning_robot_controller_and_dummy_cgv.launch +++ b/launch/cleaning/cleaning_robot_controller_and_dummy_cgv.launch @@ -1,14 +1,4 @@ <launch> - - <arg name="robot_ip" default="172.31.1.13" /> - <arg name="load_gripper" default="true" /> - - <include file="$(find ccf)/launch/cleaning/_robot_setup.launch" > - <arg name="robot_ip" value="$(arg robot_ip)" /> - <arg name="load_gripper" value="$(arg load_gripper)"/> - </include> - - <node pkg="ccf" type="moveit_cgv_controller" name="moveit_cgv_controller_instance" output="screen" /> <node pkg="ccf" type="dummy_cgv" name="my_dummy_cgv" output="screen" /> - -</launch> \ No newline at end of file + <include file="$(find ccf)/launch/cleaning/cleaning_robot_controller.launch" /> +</launch> diff --git a/launch/cleaning/cleaning_simulation.launch b/launch/cleaning/cleaning_simulation.launch index 60f3f7c..f9b5a6c 100644 --- a/launch/cleaning/cleaning_simulation.launch +++ b/launch/cleaning/cleaning_simulation.launch @@ -1,7 +1,13 @@ <launch> + <arg name="connection_address" default="tcp://*:6576" /> + <arg name="client_controllers" default="[]" /> + <include file="$(find ccf)/launch/cleaning/_simulation_setup.launch"/> - <node pkg="ccf" type="moveit_cgv_controller" name="moveit_cgv_controller_instance" output="screen" /> + <node pkg="ccf" type="moveit_cgv_controller" name="moveit_cgv_controller_instance" output="screen" > + <param name="connection_address" type="string" value="$(arg connection_address)" /> + <param name="client_controllers" type="yaml" value="$(arg client_controllers)" /> + </node> </launch> \ No newline at end of file diff --git a/launch/cleaning/cleaning_simulation_and_dummy_cgv.launch b/launch/cleaning/cleaning_simulation_and_dummy_cgv.launch index 2188048..96d7379 100644 --- a/launch/cleaning/cleaning_simulation_and_dummy_cgv.launch +++ b/launch/cleaning/cleaning_simulation_and_dummy_cgv.launch @@ -1,8 +1,4 @@ <launch> - - <include file="$(find ccf)/launch/cleaning/_simulation_setup.launch"/> - - <node pkg="ccf" type="dummy_cgv" name="dummy_cgv_instance" output="screen" /> - <node pkg="ccf" type="moveit_cgv_controller" name="moveit_cgv_controller_instance" output="screen" /> - -</launch> \ No newline at end of file + <node pkg="ccf" type="dummy_cgv" name="my_dummy_cgv" output="screen" /> + <include file="$(find ccf)/launch/cleaning/cleaning_simulation.launch" /> +</launch> diff --git a/src/ccf/controller/RobotArmController.cpp b/src/ccf/controller/RobotArmController.cpp index 9ea39c5..9b39591 100644 --- a/src/ccf/controller/RobotArmController.cpp +++ b/src/ccf/controller/RobotArmController.cpp @@ -44,8 +44,6 @@ Object *RobotArmController::resolveObject(const std::string &id) { void RobotArmController::sendScene() { if (scene) { // meaning if the (smart) pointer is not a nullptr - scene->SerializeAsString(); - ROS_INFO_STREAM("[ros2cgv] Sending scene with " << scene->objects().size() << " objects."); sendToAll(sendSceneTopic, scene->SerializeAsString()); sceneUpdateAction(); diff --git a/src/dummy_cgv_controller.cpp b/src/dummy_cgv_controller.cpp index 191078a..5714c2f 100644 --- a/src/dummy_cgv_controller.cpp +++ b/src/dummy_cgv_controller.cpp @@ -19,19 +19,21 @@ #include "ccf/connection/NngConnection.h" #include "ccf/util/NodeUtil.h" -const std::string NODE_NAME = "ros2cgv_dummy"; +std::string NODE_NAME; using CetiRosToolbox::getParameter; +using CetiRosToolbox::getPrivateParameter; int main(int argc, char **argv) { GOOGLE_PROTOBUF_VERIFY_VERSION; + NODE_NAME = ros::this_node::getName(); ros::init(argc, argv, NODE_NAME); ros::NodeHandle n("connector_node_ros_cgv"); // namespace where the connection_address is - std::string cgv_address = getParameter<std::string>(n, "connection_address", "tcp://*:6576"); + auto cgv_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576"); DummyRobotArmController connector{n, NODE_NAME}; @@ -41,6 +43,16 @@ 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"); + + ROS_INFO_STREAM("[" << NODE_NAME << "] Connecting to " << clientControllers.size() << " client controllers."); + for (const auto &client : clientControllers) { + ROS_INFO_STREAM("[" << NODE_NAME << "] 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")); + connector.addConnection(std::move(client_connection)); + } + connector.loadScene(ros::package::getPath("ccf") + "/config/config_scene.yaml"); Object *robot = nullptr; @@ -53,9 +65,13 @@ 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 - auto selectionMessageCallback = [¤tlyPickedBox, &connector, &robot, &selectedBox, &selectedBin]( + auto selectionMessageCallback = [¤tlyPickedBox, &connector, &robot, &selectedBox, &selectedBin, &n]( const Selection &selection) { + // forward the selection to the clients + connector.sendToAll(getParameter<std::string>(n, "topics/selection", "selection"), + selection.SerializeAsString()); + if (currentlyPickedBox.has_value()) { ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to accept selections while picking is in progress."); return; diff --git a/src/dummy_rag_controller_a.cpp b/src/dummy_rag_controller_a.cpp index f1ee30c..2d44325 100644 --- a/src/dummy_rag_controller_a.cpp +++ b/src/dummy_rag_controller_a.cpp @@ -28,6 +28,7 @@ const std::string CELL_NAME = "place-a"; const std::string NODE_NAME = "rag_connector_dummy_a"; using CetiRosToolbox::getParameter; +using CetiRosToolbox::getPrivateParameter; int main(int argc, char **argv) { @@ -37,7 +38,7 @@ int main(int argc, char **argv) { ros::NodeHandle n("ceti_connector"); // namespace - std::string cgv_address = getParameter<std::string>(n, "cgv_address", "tcp://*:6576"); + auto cgv_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576"); DummyRobotArmController connector{n, CELL_NAME}; diff --git a/src/dummy_rag_controller_b.cpp b/src/dummy_rag_controller_b.cpp index 0004ef2..464a69c 100644 --- a/src/dummy_rag_controller_b.cpp +++ b/src/dummy_rag_controller_b.cpp @@ -27,6 +27,7 @@ const std::string CELL_NAME = "place-b"; const std::string NODE_NAME = "rag_connector_dummy_b"; using CetiRosToolbox::getParameter; +using CetiRosToolbox::getPrivateParameter; int main(int argc, char **argv) { @@ -36,7 +37,7 @@ int main(int argc, char **argv) { ros::NodeHandle n("ceti_connector"); // namespace - std::string cgv_address = getParameter<std::string>(n, "cgv_address", "tcp://*:6576"); + auto cgv_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576"); std::string cell_prefix = getParameter(n, "place_b_prefix", CELL_NAME); diff --git a/src/moveit_cgv_controller.cpp b/src/moveit_cgv_controller.cpp index 58cb659..d38f637 100644 --- a/src/moveit_cgv_controller.cpp +++ b/src/moveit_cgv_controller.cpp @@ -18,19 +18,21 @@ #include "ccf/connection/NngConnection.h" #include "ccf/util/NodeUtil.h" -const std::string NODE_NAME = "ros2cgv_moveit"; +std::string NODE_NAME; using CetiRosToolbox::getParameter; +using CetiRosToolbox::getPrivateParameter; int main(int argc, char **argv) { GOOGLE_PROTOBUF_VERIFY_VERSION; + NODE_NAME = ros::this_node::getName(); ros::init(argc, argv, NODE_NAME); ros::NodeHandle n("connector_node_ros_cgv"); // namespace where the connection_address is - std::string cgv_address = getParameter<std::string>(n, "connection_address", "tcp://*:6576"); + auto cgv_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576"); MoveItRobotArmController connector{n, NODE_NAME}; @@ -40,6 +42,16 @@ 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"); + + ROS_INFO_STREAM("[" << NODE_NAME << "] Connecting to " << clientControllers.size() << " client controllers."); + for (const auto &client : clientControllers) { + ROS_INFO_STREAM("[" << NODE_NAME << "] 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")); + connector.addConnection(std::move(client_connection)); + } + // scene loading is not required, the scene is updated by moveit Object *robot = nullptr; @@ -52,9 +64,13 @@ 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 - auto selectionMessageCallback = [¤tlyPickedBox, &connector, &robot, &selectedBox, &selectedBin]( + auto selectionMessageCallback = [¤tlyPickedBox, &connector, &robot, &selectedBox, &selectedBin, &n]( const Selection &selection) { + // forward the selection to the clients + connector.sendToAll(getParameter<std::string>(n, "topics/selection", "selection"), + selection.SerializeAsString()); + if (currentlyPickedBox.has_value()) { ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to accept selections while picking is in progress."); return; -- GitLab