diff --git a/config/config.yaml b/config/config.yaml index 97af7cc5fff92078f36a216b2c48870529fbb16c..0056a34c3fe2d9622925fc12a6a799a835657619 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -1,5 +1,6 @@ connector_node_ros_cgv: - cgv_address: "tcp://*:6576" + topics: + selection: "selection" max_grasp_approach_velocity: 0.2 max_grasp_approach_acceleration: 0.2 max_grasp_transition_velocity: 0.2 diff --git a/include/ccf/connection/Connection.h b/include/ccf/connection/Connection.h index 1330d06ffa62cea86644b33a9eb87e9ff777019b..93e80f046e11d2c18d5de77e68ed4c2c6205278b 100644 --- a/include/ccf/connection/Connection.h +++ b/include/ccf/connection/Connection.h @@ -2,8 +2,8 @@ // Created by Johannes Mey on 28/04/2021. // -#ifndef CGV_CONNECTOR_WORKSPACE_CONNECTION_H -#define CGV_CONNECTOR_WORKSPACE_CONNECTION_H +#ifndef CCF_CONNECTION_H +#define CCF_CONNECTION_H #include <string> #include <functional> @@ -34,4 +34,4 @@ public: virtual bool send(const std::string &channel, const std::string &message) = 0; }; -#endif //CGV_CONNECTOR_WORKSPACE_CONNECTION_H +#endif //CCF_CONNECTION_H diff --git a/include/ccf/connection/MqttConnection.h b/include/ccf/connection/MqttConnection.h index 63d2131c86b4725e05e62abcc4581f095dc6696c..e86f49c1ab87a8f21dbb62a78c69c716a5c86e8d 100644 --- a/include/ccf/connection/MqttConnection.h +++ b/include/ccf/connection/MqttConnection.h @@ -2,8 +2,8 @@ // Created by Sebastian Ebert on 04.05.21. // -#ifndef CGV_SIM_MQTTCONNECTION_H -#define CGV_SIM_MQTTCONNECTION_H +#ifndef CCF_MQTTCONNECTION_H +#define CCF_MQTTCONNECTION_H #include "Connection.h" @@ -40,4 +40,4 @@ private: bool ensureConnection(); }; -#endif //CGV_SIM_MQTTCONNECTION_H \ No newline at end of file +#endif //CCF_MQTTCONNECTION_H \ No newline at end of file diff --git a/include/ccf/connection/NngConnection.h b/include/ccf/connection/NngConnection.h index 17efd7f65dba57fc2ac94ca2642940023fdacdf6..f332750094fb5b80bb621b8e25348884d240a3c3 100644 --- a/include/ccf/connection/NngConnection.h +++ b/include/ccf/connection/NngConnection.h @@ -2,8 +2,8 @@ // Created by jm on 28/04/2021. // -#ifndef CGV_CONNECTOR_WORKSPACE_NNGCONNECTION_H -#define CGV_CONNECTOR_WORKSPACE_NNGCONNECTION_H +#ifndef CCF_NNGCONNECTION_H +#define CCF_NNGCONNECTION_H #include "Connection.h" @@ -15,7 +15,8 @@ /// see https://nng.nanomsg.org/man/v0.2.0/nng_pair.html class NngConnection : public Connection { - const std::string &cgv_address; + const std::string &connection_address; + const bool server; nng_socket sock{}; std::unique_ptr<std::thread> nng_receiver_thread; std::string sendTopic; @@ -23,8 +24,10 @@ class NngConnection : public Connection { public: /// Constructor requiring an address as defined in https://nng.nanomsg.org/man/v1.3.2/nng.7.html - /// \param cgvAddress - explicit NngConnection(const std::string &cgvAddress); + /// \param connection_address in server mode the port on which the connections listens for connections, in client + /// mode the address of the server + /// \param server true, if the connection is a server (listens for connections), otherwise client mode is enabled + explicit NngConnection(const std::string &connection_address, bool server = true); bool send(const std::string &channel, const std::string &message) override; @@ -50,4 +53,4 @@ public: }; -#endif //CGV_CONNECTOR_WORKSPACE_NNGCONNECTION_H +#endif //CCF_NNGCONNECTION_H diff --git a/include/ccf/controller/Controller.h b/include/ccf/controller/Controller.h index 113cbbec2a208304f1527eb66d6a3ccf22520cec..9c22b96bc8f1d74ecb5b7cad53a4fa372f4d3046 100644 --- a/include/ccf/controller/Controller.h +++ b/include/ccf/controller/Controller.h @@ -2,8 +2,8 @@ // Created by Johannes Mey on 17/01/2021. // -#ifndef CGV_CONNECTOR_WORKSPACE_CONTROLLER_H -#define CGV_CONNECTOR_WORKSPACE_CONTROLLER_H +#ifndef CCF_CONTROLLER_H +#define CCF_CONTROLLER_H #include <functional> #include <optional> @@ -35,4 +35,4 @@ public: }; -#endif //CGV_CONNECTOR_WORKSPACE_CONTROLLER_H +#endif //CCF_CONTROLLER_H diff --git a/include/ccf/controller/DummyRobotArmController.h b/include/ccf/controller/DummyRobotArmController.h index 7cac02482245c15cf3ad18ac5dbdf01fd181596d..252c95d7615b383b8608d28e8143ed38fb49ce17 100644 --- a/include/ccf/controller/DummyRobotArmController.h +++ b/include/ccf/controller/DummyRobotArmController.h @@ -2,8 +2,8 @@ // Created by Johannes Mey on 17/01/2021. // -#ifndef CGV_CONNECTOR_WORKSPACE_DUMMYROBOTARMCONTROLLER_H -#define CGV_CONNECTOR_WORKSPACE_DUMMYROBOTARMCONTROLLER_H +#ifndef CCF_DUMMYROBOTARMCONTROLLER_H +#define CCF_DUMMYROBOTARMCONTROLLER_H #include "RobotArmController.h" @@ -33,4 +33,4 @@ public: virtual bool reachableObject(const Object &robot, const Object &object) override; }; -#endif //CGV_CONNECTOR_WORKSPACE_DUMMYROBOTARMCONTROLLER_H +#endif //CCF_DUMMYROBOTARMCONTROLLER_H diff --git a/include/ccf/controller/MoveItRobotArmController.h b/include/ccf/controller/MoveItRobotArmController.h index 0b0066a10b10cfee78ce556fa8181ea48ea00cdb..9c745c29add9daf2c6b9ea74a131835464a18c34 100644 --- a/include/ccf/controller/MoveItRobotArmController.h +++ b/include/ccf/controller/MoveItRobotArmController.h @@ -2,8 +2,8 @@ // Created by Johannes Mey on 17/01/2021. // -#ifndef CGV_CONNECTOR_WORKSPACE_MOVEITROBOTARMCONTROLLER_H -#define CGV_CONNECTOR_WORKSPACE_MOVEITROBOTARMCONTROLLER_H +#ifndef CCF_MOVEITROBOTARMCONTROLLER_H +#define CCF_MOVEITROBOTARMCONTROLLER_H #include <ccf/SceneUpdate.h> @@ -35,4 +35,4 @@ public: }; -#endif //CGV_CONNECTOR_WORKSPACE_MOVEITROBOTARMCONTROLLER_H +#endif //CCF_MOVEITROBOTARMCONTROLLER_H diff --git a/include/ccf/controller/RobotArmController.h b/include/ccf/controller/RobotArmController.h index f3d047eed5ce0312408a7f8ef4f9a18a67d6441c..00621eaecdfd38e1465b25cc2b40e417a19009d5 100644 --- a/include/ccf/controller/RobotArmController.h +++ b/include/ccf/controller/RobotArmController.h @@ -2,8 +2,8 @@ // Created by Johannes Mey on 17/01/2021. // -#ifndef CGV_CONNECTOR_WORKSPACE_ROBOTARMCONTROLLER_H -#define CGV_CONNECTOR_WORKSPACE_ROBOTARMCONTROLLER_H +#ifndef CCF_ROBOTARMCONTROLLER_H +#define CCF_ROBOTARMCONTROLLER_H #include <functional> #include <optional> @@ -95,4 +95,4 @@ public: Object *resolveObject(const std::string &id); }; -#endif //CGV_CONNECTOR_WORKSPACE_ROBOTARMCONTROLLER_H +#endif //CCF_ROBOTARMCONTROLLER_H diff --git a/include/ccf/util/NodeUtil.h b/include/ccf/util/NodeUtil.h index 200d33ef873f8272393bdfd2271ff34a4ed5e699..845a8a3e72e8feb03e179c912d7f1d67192418e7 100644 --- a/include/ccf/util/NodeUtil.h +++ b/include/ccf/util/NodeUtil.h @@ -44,6 +44,34 @@ namespace CetiRosToolbox { return fallback; } + /// Gets a private parameter from the parameter server. If the parameter does not exist, the fallback is used. + /// If no fallback value is specified, an empty string is returned. + /// + /// If the parameter is \a not on the parameter server, a warning is issued at \c ROS_WARN. + /// + /// Usually, when using the method, the template parameter can be omitted if a fallback value is provided. This does + /// \a not work when providing \c const \c char* as fallback (i.e., string constants). In this case, the template + /// parameter must be provided: + /// \code{.cpp} + /// auto intValue1 = getParameter<int>(n, "foo"); // no fallback means no automatic matching + /// auto intValue2 = getParameter(n, "foo", 42); // the compiler matches T with int, this is okay + /// auto stringValue = getParameter<std::string>(n, "bar", "fourtytwo"); // const char* requires the std::string template parameter + /// \endcode + /// \tparam T a type supported by the parameter server. + /// The parameter server supports \c std::string double \c float \c int \c bool + /// and \c std::vector and \std::map of these types. + /// Additionally, also \c XmlRpc::XmlRpcValue is supported, though not as a vector or map. + /// \param key the name of the parameter + /// \param fallback the default value + /// \return the parameter (if it exists), otherwise the fallback value (if it exists), otherwise the default + /// instance of the respective object + /// \warning Currently, this is a header-only library, but this might change in the future + template<class T> + T getPrivateParameter(const std::string &key, T fallback = T()) { + ros::NodeHandle n("~"); // a private node handle + return getParameter(n, key, fallback); + } + /** @cond */ // the specializations are required to print the warning correctly template<> XmlRpc::XmlRpcValue getParameter(const ros::NodeHandle &n, const std::string &key, XmlRpc::XmlRpcValue fallback) { diff --git a/include/ccf/util/scene_collision_object.h b/include/ccf/util/scene_collision_object.h index 4857e997578d74edbe1ce8177bdcf601b5e1438b..55b851bc3f30519861498861b815b3e8339e98c5 100644 --- a/include/ccf/util/scene_collision_object.h +++ b/include/ccf/util/scene_collision_object.h @@ -2,8 +2,8 @@ // Created by sebastian on 14.01.21. // -#ifndef CGV_CONNECTOR_SCENECOLLISIONOBJECT_H -#define CGV_CONNECTOR_SCENECOLLISIONOBJECT_H +#ifndef CCF_SCENECOLLISIONOBJECT_H +#define CCF_SCENECOLLISIONOBJECT_H #include <moveit/planning_scene_interface/planning_scene_interface.h> #include <moveit/move_group_interface/move_group_interface.h> @@ -37,4 +37,4 @@ private: }; -#endif //CGV_CONNECTOR_SCENECOLLISIONOBJECT_H +#endif //CCF_SCENECOLLISIONOBJECT_H diff --git a/include/ccf/util/scene_constructor_util.h b/include/ccf/util/scene_constructor_util.h index a680d152c9734c055aa2e236aa6ecaad27e8806c..7b73381291dcaca038580984af40e66160fb5ea9 100644 --- a/include/ccf/util/scene_constructor_util.h +++ b/include/ccf/util/scene_constructor_util.h @@ -2,8 +2,8 @@ // Created by Sebastian Ebert on 03.08.20. // -#ifndef CGV_CONNECTOR_SCENE_CONSTRUCTOR_H -#define CGV_CONNECTOR_SCENE_CONSTRUCTOR_H +#ifndef CCF_SCENE_CONSTRUCTOR_H +#define CCF_SCENE_CONSTRUCTOR_H #include <moveit/planning_scene_interface/planning_scene_interface.h> @@ -55,4 +55,4 @@ public: std::vector<std_msgs::ColorRGBA> &collision_objects_colors, const Scene &scene); }; -#endif //CGV_CONNECTOR_SCENE_CONSTRUCTOR_H \ No newline at end of file +#endif //CCF_SCENE_CONSTRUCTOR_H \ No newline at end of file diff --git a/launch/cleaning/cleaning_dummy_controller.launch b/launch/cleaning/cleaning_dummy_controller.launch index 3b28246a90a412e9fd14db9c2054bcfa2e568350..7ce2c9c6d21d609514c386376aad87e93c6b1ebe 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 ebb269c59dd0bbaafae583b58e66b6b6f094fbc7..0caf4848393fd80c6b1d78b9aad651df2db7de1b 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_dummy_main_dummy_client_dummy_cgv.launch b/launch/cleaning/cleaning_dummy_main_dummy_client_dummy_cgv.launch new file mode 100644 index 0000000000000000000000000000000000000000..8bb339e9dab05f69708e79b02bad4883d7eb75b1 --- /dev/null +++ b/launch/cleaning/cleaning_dummy_main_dummy_client_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="main_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="client_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_main_simulated_client.launch b/launch/cleaning/cleaning_dummy_main_simulated_client.launch new file mode 100644 index 0000000000000000000000000000000000000000..8ccce6177645371fe60c722888599e7f0f68d7c6 --- /dev/null +++ b/launch/cleaning/cleaning_dummy_main_simulated_client.launch @@ -0,0 +1,13 @@ +<launch> + <include file="$(find ccf)/launch/cleaning/_simulation_setup.launch"/> + + <!--<node pkg="ccf" type="dummy_cgv" name="my_dummy_cgv" output="screen" />--> + <node pkg="ccf" type="moveit_cgv_controller" name="main_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="client_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_robot_controller.launch b/launch/cleaning/cleaning_robot_controller.launch index a44470f58df938f65dfeca90c1567964050519c2..206780e84ea419d32aa204f1925490fdcce37c5c 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 d5e84b2b4f7df7173590eb8080f29c902dd4431f..190cb8536bccc44f0ae5cdae86319f72f29a829b 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_simulated_main_dummy_client.launch b/launch/cleaning/cleaning_simulated_main_dummy_client.launch new file mode 100644 index 0000000000000000000000000000000000000000..ef9f1ebc13c8c8c2f009287c82772eef8a2a81c5 --- /dev/null +++ b/launch/cleaning/cleaning_simulated_main_dummy_client.launch @@ -0,0 +1,13 @@ +<launch> + <include file="$(find ccf)/launch/cleaning/_simulation_setup.launch"/> + + <!--<node pkg="ccf" type="dummy_cgv" name="my_dummy_cgv" output="screen" />--> + <node pkg="ccf" type="dummy_cgv_controller" name="main_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="moveit_cgv_controller" name="client_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_simulation.launch b/launch/cleaning/cleaning_simulation.launch index 60f3f7ce950292a70e50ad1fddbcf07f0c265489..f9b5a6cd9961d5da418ce24fc28302166b21ae92 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 218804860ea27a429a200b2f6f0cc5f2e4179cff..96d7379a184345a50379497e10b5cb737fc78e72 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/connection/NngConnection.cpp b/src/ccf/connection/NngConnection.cpp index a1cab0490e12fc3c81a8280b6d86c44813af4627..0ace0b248f5cd54f80dd36a62f3160240f7cee03 100644 --- a/src/ccf/connection/NngConnection.cpp +++ b/src/ccf/connection/NngConnection.cpp @@ -28,7 +28,7 @@ void NngConnection::receiveMessages() { } else if (recv_rv == NNG_EAGAIN) { // no message received in current spin } else { - ROS_ERROR_STREAM("[ros2cgv] nng_recv returned: " << nng_strerror(recv_rv)); + ROS_ERROR_STREAM("[" << ros::this_node::getName() << "] nng_recv returned: " << nng_strerror(recv_rv)); } loop_rate.sleep(); } @@ -37,32 +37,50 @@ void NngConnection::receiveMessages() { bool NngConnection::initializeConnection(std::function<void(std::string, std::string)> callback) { + int rv; - // first, establish the connection - ROS_INFO_STREAM("[ros2cgv] listening for connections at " << cgv_address); + if (server) { + // first, establish the connection + ROS_INFO_STREAM("[NngConnection] listening for connections at " << connection_address); - int rv; - if ((rv = nng_pair1_open(&sock)) != 0) { - ROS_ERROR_STREAM("[ros2cgv] nng_pair1_open returned: " << nng_strerror(rv)); - return false; - } + if ((rv = nng_pair1_open(&sock)) != 0) { + ROS_ERROR_STREAM("[NngConnection] nng_pair1_open returned: " << nng_strerror(rv)); + return false; + } - nng_listener listener; - if ((rv = nng_listen(sock, cgv_address.c_str(), &listener, 0)) != 0) { - ROS_ERROR_STREAM("[ros2cgv] nng_listen returned: " << nng_strerror(rv)); - return false; - } + nng_listener listener; + if ((rv = nng_listen(sock, connection_address.c_str(), &listener, 0)) != 0) { + ROS_ERROR_STREAM("[NngConnection] nng_listen returned: " << nng_strerror(rv)); + return false; + } + + // then, set the callback + messageReceivedCallback = callback; - // then, set the callback - messageReceivedCallback = callback; + // then, start the thread that uses the callback + nng_receiver_thread = std::make_unique<std::thread>(&NngConnection::receiveMessages, this); + } else { // client mode + + ROS_INFO_STREAM("[NngConnection] establishing connection with " << connection_address); + if ((rv = nng_pair1_open(&sock)) != 0) { + ROS_ERROR_STREAM("[NngConnection] nng_pair1_open returned: " << nng_strerror(rv)); + } + + ros::Rate connection_retry_rate(1); + while ((rv = nng_dial(sock, connection_address.c_str(), nullptr, 0)) != 0) { + ROS_WARN_STREAM("[NngConnection] nng_dial returned: " << nng_strerror(rv) + << ". Trying to connect again in one second..."); + connection_retry_rate.sleep(); + } + ROS_INFO_STREAM("[NngConnection] Connection established!"); + } - // then, start the thread that uses the callback - nng_receiver_thread = std::make_unique<std::thread>(&NngConnection::receiveMessages, this); return true; } -NngConnection::NngConnection(const std::string &cgvAddress) : cgv_address{cgvAddress}, sock{0} {} +NngConnection::NngConnection(const std::string &connection_address, bool server) : + connection_address{connection_address}, sock{0}, server{server} {} bool NngConnection::send(const std::string &channel, const std::string &message) { diff --git a/src/ccf/controller/RobotArmController.cpp b/src/ccf/controller/RobotArmController.cpp index 9ea39c59b97a8d89ed1d63c4403025dc7cc91d1b..9b39591609db4cc91cf15a112e7e5b602ce1255d 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 a05133b0cfa3dcc651b910f6aaf954dce29a7db0..8e03d60c03825c6ee3e5b7794ece422641b0ac53 100644 --- a/src/dummy_cgv_controller.cpp +++ b/src/dummy_cgv_controller.cpp @@ -19,23 +19,21 @@ #include "ccf/connection/NngConnection.h" #include "ccf/util/NodeUtil.h" -const std::string NODE_NAME = "ros2cgv_dummy"; +std::string NODE_NAME = "dummy_cgv_controller"; using CetiRosToolbox::getParameter; +using CetiRosToolbox::getPrivateParameter; int main(int argc, char **argv) { GOOGLE_PROTOBUF_VERIFY_VERSION; ros::init(argc, argv, NODE_NAME); + NODE_NAME = ros::this_node::getName(); - ros::NodeHandle n("connector_node_ros_cgv"); // namespace where the cgv_address is + ros::NodeHandle n("connector_node_ros_cgv"); // namespace where the connection_address is - std::string cgv_address = "tcp://*:6576"; - if (!n.getParam("cgv_address", cgv_address)) { - ROS_WARN_STREAM("[" << NODE_NAME << "] Could not get string value for " << n.getNamespace() - << "/cgv_address from param server, using default " << cgv_address); - } + auto cgv_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576"); DummyRobotArmController connector{n, NODE_NAME}; @@ -45,6 +43,17 @@ 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")); + client_connection->setReceiveTopic("client_scene"); + connector.addConnection(std::move(client_connection)); + } + connector.loadScene(ros::package::getPath("ccf") + "/config/config_scene.yaml"); Object *robot = nullptr; @@ -57,9 +66,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; @@ -118,4 +131,4 @@ int main(int argc, char **argv) { ros::spin(); return 0; -} \ No newline at end of file +} diff --git a/src/dummy_rag_controller_a.cpp b/src/dummy_rag_controller_a.cpp index f1ee30c697f28ef5c183553221b0865ae6aaa5ad..2d44325f68f9b9e3c43e835c208bdd2ded372c37 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 0004ef290fa7d42533c9f87b2190daa729f8bea0..464a69c28919feadc976156c2e40eb1c6e1c7d24 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 296b943d8b94300fd3279bf33ac0db95db3973a8..c8b548cd4aaaec5042e6aa101f6140bbed225ff8 100644 --- a/src/moveit_cgv_controller.cpp +++ b/src/moveit_cgv_controller.cpp @@ -18,23 +18,21 @@ #include "ccf/connection/NngConnection.h" #include "ccf/util/NodeUtil.h" -const std::string NODE_NAME = "ros2cgv_moveit"; +std::string NODE_NAME = "moveit_cgv_controller"; using CetiRosToolbox::getParameter; +using CetiRosToolbox::getPrivateParameter; int main(int argc, char **argv) { GOOGLE_PROTOBUF_VERIFY_VERSION; ros::init(argc, argv, NODE_NAME); + NODE_NAME = ros::this_node::getName(); - ros::NodeHandle n("connector_node_ros_cgv"); // namespace where the cgv_address is + ros::NodeHandle n("connector_node_ros_cgv"); // namespace where the connection_address is - std::string cgv_address = "tcp://*:6576"; - if (!n.getParam("cgv_address", cgv_address)) { - ROS_WARN_STREAM("[" << NODE_NAME << "] Could not get string value for " << n.getNamespace() - << "/cgv_address from param server, using default " << cgv_address); - } + auto cgv_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576"); MoveItRobotArmController connector{n, NODE_NAME}; @@ -44,6 +42,17 @@ 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")); + client_connection->setReceiveTopic("client_scene"); + connector.addConnection(std::move(client_connection)); + } + // scene loading is not required, the scene is updated by moveit Object *robot = nullptr; @@ -56,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;