diff --git a/src/dummy_cgv_controller.cpp b/src/dummy_cgv_controller.cpp index 70e5de991e00ff37fe7f98bd6ba1e44e2505f44f..d89c9c25015776a1f332bdbbca07877a78ac57ae 100644 --- a/src/dummy_cgv_controller.cpp +++ b/src/dummy_cgv_controller.cpp @@ -19,12 +19,15 @@ #include "ccf/controller/DummyRobotArmController.h" #include "ccf/connection/NngConnection.h" +#include "ccf/util/NodeUtil.h" #include <google/protobuf/text_format.h> #include <google/protobuf/util/json_util.h> const std::string NODE_NAME = "ros2cgv_dummy"; +using CetiRosToolbox::getParameter; + void loadScene(DummyRobotArmController &connector) { // read file into string: https://stackoverflow.com/questions/2602013/read-whole-ascii-file-into-c-stdstring std::string path = ros::package::getPath("ccf") + "/config/config_scene.yaml"; @@ -74,22 +77,12 @@ int main(int argc, char **argv) { << "/cgv_address from param server, using default " << cgv_address); } - DummyRobotArmController connector{n, "place-a"}; + DummyRobotArmController connector{n, NODE_NAME}; // add an NNG connection std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(cgv_address); - std::string selectionTopic = "selection"; - if (!n.getParam("topics/selection", selectionTopic)) { - ROS_WARN_STREAM("[" << NODE_NAME << "] Could not get string value for " << n.getNamespace() - << "/topics/selection from param server, using default " << selectionTopic); - } - connection->setReceiveTopic(selectionTopic); - std::string sceneTopic = "scene"; - if (!n.getParam("topics/scene", sceneTopic)) { - ROS_WARN_STREAM("[" << NODE_NAME << "] Could not get string value for " << n.getNamespace() - << "/topics/scene from param server, using default " << sceneTopic); - } - connection->setSendTopic(sceneTopic); + connection->setReceiveTopic(getParameter(n, "topics/selection", "selection")); + connection->setSendTopic(getParameter(n, "topics/scene", NODE_NAME + "/scene/update")); connector.addConnection(std::move(connection)); loadScene(connector); @@ -126,7 +119,7 @@ int main(int argc, char **argv) { if (selectedBin && selectedBox) { auto boxId = selectedBox->id(); ROS_INFO_STREAM("[" << NODE_NAME << "] Got task: Put " << boxId << " into " << selectedBin->id()); - if (!connector.RobotArmController::pickAndDrop(*robot, *selectedBox, *selectedBin, false)) { + if (!connector.pickAndDrop(*robot, *selectedBox, *selectedBin, false)) { ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to remove box '" << boxId << "'!"); selectedBox = nullptr; selectedBin = nullptr; diff --git a/src/moveit_cgv_controller.cpp b/src/moveit_cgv_controller.cpp index 9fb57135a558185fededb00dd78c6a03191e50d8..afe697f3e7771c52a4562a48b07a58641a184fbd 100644 --- a/src/moveit_cgv_controller.cpp +++ b/src/moveit_cgv_controller.cpp @@ -20,9 +20,12 @@ #include "ccf/controller/MoveItRobotArmController.h" #include "ccf/connection/NngConnection.h" +#include "ccf/util/NodeUtil.h" const std::string NODE_NAME = "ros2cgv_moveit"; +using CetiRosToolbox::getParameter; + int main(int argc, char **argv) { GOOGLE_PROTOBUF_VERIFY_VERSION; @@ -37,25 +40,17 @@ int main(int argc, char **argv) { << "/cgv_address from param server, using default " << cgv_address); } - MoveItRobotArmController connector{n, "place-a"}; + MoveItRobotArmController connector{n, NODE_NAME}; // add an NNG connection std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(cgv_address); - std::string selectionTopic = "selection"; - if (!n.getParam("topics/selection", selectionTopic)) { - ROS_WARN_STREAM("[" << NODE_NAME << "] Could not get string value for " << n.getNamespace() - << "/topics/selection from param server, using default " << selectionTopic); - } - connection->setReceiveTopic(selectionTopic); - std::string sceneTopic = "scene"; - if (!n.getParam("topics/scene", sceneTopic)) { - ROS_WARN_STREAM("[" << NODE_NAME << "] Could not get string value for " << n.getNamespace() - << "/topics/scene from param server, using default " << sceneTopic); - } - connection->setSendTopic(sceneTopic); + connection->setReceiveTopic(getParameter(n, "topics/selection", "selection")); + connection->setSendTopic(getParameter(n, "topics/scene", NODE_NAME + "/scene/update")); connector.addConnection(std::move(connection)); - std::shared_ptr<Object> robot{}; + // scene loading is not required, the scene is updated by moveit + + Object* robot = nullptr; Object* selectedBox = nullptr; Object* selectedBin = nullptr; std::optional<std::string> currentlyPickedBox{}; @@ -87,14 +82,16 @@ int main(int argc, char **argv) { if (selectedBin && selectedBox) { auto boxId = selectedBox->id(); ROS_INFO_STREAM("[" << NODE_NAME << "] Got task: Put " << boxId << " into " << selectedBin->id()); - if (!connector.RobotArmController::pickAndDrop(*robot, *selectedBox, *selectedBin, false)) { + if (!connector.pickAndDrop(*robot, *selectedBox, *selectedBin, false)) { ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to remove box '" << boxId << "'!"); + selectedBox = nullptr; + selectedBin = nullptr; } else { ROS_INFO_STREAM("[" << NODE_NAME << "] Job is done! '" << boxId << "' is no more."); + selectedBox = nullptr; + selectedBin = nullptr; connector.sendScene(); } - selectedBox = nullptr; - selectedBin = nullptr; } }; @@ -102,7 +99,7 @@ int main(int argc, char **argv) { ros::ServiceClient bin_check_client = n.serviceClient<ccf::BinCheck>("/check_bin_service"); ros::ServiceServer get_scene_service = n.advertiseService<ccf::SceneUpdateRequest, ccf::SceneUpdateResponse>( - "/connector_node_ros_cgv/updateCgvScene", + "updateCgvScene", [¤tlyPickedBox, &connector, &bin_check_client](auto &req, auto &res) { ROS_INFO_STREAM("[" << NODE_NAME << "] Received a scene update from the controller."); connector.updateScene(req, bin_check_client); @@ -119,4 +116,4 @@ int main(int argc, char **argv) { ros::spin(); return 0; -} +} \ No newline at end of file