add new debugging controller node

CMakeLists.txt

@@ -135,6 +135,7 @@ include_directories(
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
 add_executable(${PROJECT_NAME}_dummy_selection_provider src/dummy_selection_provider.cpp ${PROTO_SRCS} ${PROTO_HDRS})
+add_executable(${PROJECT_NAME}_dummy_pickplace_provider src/dummy_pickplace_provider.cpp ${PROTO_SRCS} ${PROTO_HDRS})
 add_executable(${PROJECT_NAME}_dummy_sorting_controller src/dummy_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS})
 add_executable(${PROJECT_NAME}_main_controller src/main_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS} src/cell.cpp src/action.cpp src/task.cpp src/cell_controller.cpp include/cell_controller.h)
 add_executable(${PROJECT_NAME}_moveit_sorting_controller src/moveit_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS})
@@ -145,6 +146,7 @@ add_executable(${PROJECT_NAME}_moveit_sorting_controller src/moveit_sorting_cont
 ## target back to the shorter version for ease of user use
 ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
 set_target_properties(${PROJECT_NAME}_dummy_selection_provider PROPERTIES OUTPUT_NAME dummy_selection_provider PREFIX "")
+set_target_properties(${PROJECT_NAME}_dummy_pickplace_provider PROPERTIES OUTPUT_NAME dummy_pickplace_provider PREFIX "")
 set_target_properties(${PROJECT_NAME}_dummy_sorting_controller PROPERTIES OUTPUT_NAME dummy_sorting_controller PREFIX "")
 set_target_properties(${PROJECT_NAME}_main_controller PROPERTIES OUTPUT_NAME main_controller PREFIX "")
 set_target_properties(${PROJECT_NAME}_moveit_sorting_controller PROPERTIES OUTPUT_NAME moveit_sorting_controller PREFIX "")
@@ -152,6 +154,7 @@ set_target_properties(${PROJECT_NAME}_moveit_sorting_controller PROPERTIES OUTPU
 ## Add cmake target dependencies of the executable
 ## same as for the library above
 add_dependencies(${PROJECT_NAME}_dummy_selection_provider ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME}_dummy_pickplace_provider ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(${PROJECT_NAME}_dummy_sorting_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(${PROJECT_NAME}_main_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(${PROJECT_NAME}_moveit_sorting_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
@@ -170,6 +173,9 @@ target_link_libraries(${PROJECT_NAME}_dummy_selection_provider
         ${catkin_LIBRARIES}
         nng
         )
+target_link_libraries(${PROJECT_NAME}_dummy_pickplace_provider
+        ${catkin_LIBRARIES}
+        )
 
 
 #############

src/dummy_pickplace_provider.cpp

+/*! \file dummy_selection_provider.cpp
+
+    A ROS node that can trigger pick-place and pick-drop commands on cells
+
+    \author Johannes Mey
+    \date 26.04.2022
+*/
+
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+
+#include "ccf/util/NodeUtil.h"
+
+#include "connector.pb.h"
+
+#include <google/protobuf/message.h>
+
+#include "ccf/connection/MqttConnection.h"
+
+using CetiRosToolbox::getPrivateParameter;
+
+int main(int argc, char** argv)
+{
+  GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+  ros::init(argc, argv, "dummy_pickplace_provider");
+
+  ros::NodeHandle n("ccf");
+
+  auto mqtt_server = getPrivateParameter<std::string>("mqtt_server", "tcp://127.0.0.1:1883");
+  auto prefix = CetiRosToolbox::getPrivateParameter<std::string>("prefix", "sub_select");
+  auto target_cell = CetiRosToolbox::getPrivateParameter<std::string>("target_cell", "ceti_cell_placeworld");
+
+  std::shared_ptr<MqttConnection> client_connection =
+      std::make_shared<MqttConnection>(mqtt_server, ros::this_node::getName());
+
+  client_connection->initializeConnection(
+      [](auto t, auto m) { ROS_WARN_STREAM("ignoring message " << m << " on topic " << t); });
+
+  ros::Subscriber sub_select = n.subscribe<std_msgs::String>(prefix + "/pp", 1000, [&](auto& m) {
+    std::string command = m->data;
+
+    ROS_INFO_STREAM("Selecting pick/place command: " << command);
+    PickPlace p;
+
+    size_t pos = command.find(',');
+    p.set_idpick(command.substr(0, pos));
+    command.erase(0, pos + 1);
+    pos = command.find(',');
+    p.set_idplace(command.substr(0, pos));
+    command.erase(0, pos + 1);
+    p.set_idrobot(command);
+
+    ROS_INFO_STREAM("sending command for robot " << p.idrobot() << " to place " << p.idpick() << " at " << p.idplace());
+    client_connection->send("/" + target_cell + "/command", p.SerializeAsString());
+  });
+
+  ROS_INFO_STREAM("DUMMY PICK-PLACE-PROVIDER" << std::endl
+                                              << "To trigger a commend, use" << std::endl
+                                              << "  rostopic pub -1 /ccf/" << prefix
+                                              << "/pp std_msgs/String \"<object>,<target>,<robot>\"");
+
+  ros::spin();
+
+  return 0;
+}
\ No newline at end of file