fix execution bug, simplyfy parameter handling

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;

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.");
-                connector.sendScene();
-            }
                 selectedBox = nullptr;
                 selectedBin = nullptr;
+                connector.sendScene();
+            }
         }
     };
 
@@ -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",
             [&currentlyPickedBox, &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);