add virtual scene provider

CMakeLists.txt

@@ -138,6 +138,7 @@ add_executable(${PROJECT_NAME}_dummy_selection_provider src/dummy_selection_prov
 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}_virtual_scene_provider src/virtual_scene_provider.cpp ${PROTO_SRCS} ${PROTO_HDRS})
 add_executable(${PROJECT_NAME}_moveit_sorting_controller src/moveit_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS})
 add_executable(${PROJECT_NAME}_object_locator src/object_locator.cpp ${PROTO_SRCS} ${PROTO_HDRS})
 
@@ -150,6 +151,7 @@ set_target_properties(${PROJECT_NAME}_dummy_selection_provider PROPERTIES OUTPUT
 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}_virtual_scene_provider PROPERTIES OUTPUT_NAME virtual_scene_provider PREFIX "")
 set_target_properties(${PROJECT_NAME}_moveit_sorting_controller PROPERTIES OUTPUT_NAME moveit_sorting_controller PREFIX "")
 set_target_properties(${PROJECT_NAME}_object_locator PROPERTIES OUTPUT_NAME object_locator PREFIX "")
 
@@ -159,6 +161,7 @@ add_dependencies(${PROJECT_NAME}_dummy_selection_provider ${${PROJECT_NAME}_EXPO
 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}_virtual_scene_provider ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(${PROJECT_NAME}_moveit_sorting_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(${PROJECT_NAME}_object_locator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
@@ -169,6 +172,9 @@ target_link_libraries(${PROJECT_NAME}_dummy_sorting_controller
 target_link_libraries(${PROJECT_NAME}_main_controller
         ${catkin_LIBRARIES}
         )
+target_link_libraries(${PROJECT_NAME}_virtual_scene_provider
+        ${catkin_LIBRARIES}
+        )
 target_link_libraries(${PROJECT_NAME}_moveit_sorting_controller
         ${catkin_LIBRARIES}
         )

launch/distributed-sorting/virtual_scene_provider.launch

+<launch>
+
+    <arg name="connection_address" default="tcp://*:6576" doc="connection address for NNG scene-based selection"/>
+    <arg name="mqtt_server" default="tcp://127.0.0.1:1883" doc="MQTT server for communication with client cells"/>
+
+    <node pkg="ccf_immersive_sorting" type="virtual_scene_provider" name="main_controller" output="screen">
+        <param name="connection_address" type="string" value="$(arg connection_address)"/>
+        <param name="mqtt_server" type="yaml" value="$(arg mqtt_server)"/>
+        <param name="arm" type="string" value="virtual-arm"/>
+        <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene_virtual-tag-table-grey.json"/>
+    </node>
+
+</launch>

src/virtual_scene_provider.cpp

+#include <ros/ros.h>
+#include <ros/package.h>
+#include <std_msgs/Empty.h>
+
+#include <utility>
+#include <cell_controller.h>
+
+#include "connector.pb.h"
+
+#include "ccf/controller/DummyRobotArmController.h"
+#include "ccf/connection/NngConnection.h"
+#include "ccf/connection/MqttConnection.h"
+#include "ccf/util/NodeUtil.h"
+
+const char *DEFAULT_NODE_NAME = "virtual_scene_provider";
+std::string NODE_NAME;
+
+using CetiRosToolbox::getParameter;
+using CetiRosToolbox::getPrivateParameter;
+
+const float UNSELECT = 0.8;
+const float SELECT = 1/UNSELECT;
+const float DELETING = 0.08;
+
+void highlight(Object *selected_bin, float factor)
+{
+  if (selected_bin)
+  {
+    selected_bin->mutable_color()->set_r(selected_bin->color().r() * factor);
+    selected_bin->mutable_color()->set_g(selected_bin->color().g() * factor);
+    selected_bin->mutable_color()->set_b(selected_bin->color().b() * factor);
+  }
+}
+
+int main(int argc, char **argv)
+{
+  GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+  ros::init(argc, argv, DEFAULT_NODE_NAME);
+  NODE_NAME = ros::this_node::getName();
+
+  ros::NodeHandle n("connector_node_ros_ccf");  // namespace where the connection_address is
+
+  auto connection_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
+
+  auto robotName = getPrivateParameter<std::string>("arm", "virtual-arm");
+  ROS_INFO_STREAM("This cell controls arm " << robotName);
+
+  DummyRobotArmController controller{n, NODE_NAME, robotName};
+
+  std::map<std::string, CellController> clients;
+  std::vector<std::pair<std::string, std::string>> actions;
+
+  // add an NNG connection
+  std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(connection_address);
+  connection->setReceiveTopic(getParameter<std::string>(n, "topics/selection", "selection"));
+  connection->setSendTopic(getParameter(n, "topics/scene", NODE_NAME + "/scene/update"));
+  controller.addConnection(std::move(connection));
+
+  auto mqtt_server = getPrivateParameter<std::string>("mqtt_server", "tcp://127.0.0.1:1883");
+  std::shared_ptr<MqttConnection> client_connection = std::make_shared<MqttConnection>(mqtt_server, NODE_NAME);
+  controller.addConnection(client_connection);
+  client_connection->listen(getParameter<std::string>(n, "topics/selection", "selection"));
+
+  auto fallback_path = ros::package::getPath("ccf_immersive_sorting") + "/config/config_scene_virtual-tag-table-grey.json";
+  controller.loadScene(getPrivateParameter<std::string>("scene", fallback_path));
+
+  Object *robot = controller.resolveObject(controller.getRobotName());
+  Object *selected_box = nullptr;
+  Object *selected_bin = nullptr;
+
+  ros::Subscriber sub = n.subscribe<std_msgs::Empty>(
+      "send_scene", 1000, [&controller](const std_msgs::EmptyConstPtr &msg)
+      { controller.sendScene(); });
+  ros::Timer timer = n.createTimer(ros::Duration(10), [&controller](const ros::TimerEvent &event)
+  {
+    controller.sendScene();
+  });  // send a scene every ten seconds
+
+  auto selection_message_callback =
+      [&controller, &selected_box, &selected_bin, client_connection](const Selection &selection)
+      {
+
+        client_connection->send("vr_selection", selection.SerializeAsString());
+
+        Object *object = controller.resolveObject(selection.id());
+        if (!object)
+        {
+          ROS_ERROR_STREAM("Selected unknown object '" << selection.id() << "'");
+          return;
+        }
+        auto type = Object::Type_Name(object->type());
+        if (object->type() == Object::BOX)
+        {
+          highlight(selected_box, UNSELECT);
+          if (selected_box == object)
+          {
+            selected_box = nullptr;
+          }
+          else
+          {
+            selected_box = object;
+            highlight(selected_box, SELECT);
+          }
+          controller.sendScene();
+          ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
+        }
+        else if (object->type() == Object::BIN)
+        {
+          highlight(selected_bin, UNSELECT);
+          if (selected_bin == object)
+          {
+            selected_bin = nullptr;
+          }
+          else
+          {
+            selected_bin = object;
+            highlight(selected_bin, SELECT);
+          }
+          controller.sendScene();
+          ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
+        }
+        else
+        {
+          ROS_WARN_STREAM("Selected unsupported " << type << " '" << selection.id() << "'");
+        }
+
+        if (selected_bin && selected_box)
+        {
+          auto boxId = selected_box->id();
+          ROS_INFO_STREAM("Got task: Put " << boxId << " into " << selected_bin->id());
+          highlight(selected_bin, UNSELECT);
+          highlight(selected_box, UNSELECT);
+          controller.sendScene();
+
+          selected_box = nullptr;
+          selected_bin = nullptr;
+        }
+      };
+  controller.reactToSelectionMessage(selection_message_callback);
+
+  ros::spin();
+
+  return 0;
+}
\ No newline at end of file