send the scene more frequently, restart the scene provider when it crashes

launch/distributed-sorting/virtual_scene_provider.launch

@@ -3,7 +3,7 @@
     <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">
+    <node pkg="ccf_immersive_sorting" type="virtual_scene_provider" name="main_controller" output="screen" respawn="true" respawn_delay="3">
         <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"/>

src/virtual_scene_provider.cpp

@@ -12,17 +12,17 @@
 #include "ccf/connection/MqttConnection.h"
 #include "ccf/util/NodeUtil.h"
 
-const char *DEFAULT_NODE_NAME = "virtual_scene_provider";
+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 SELECT = 1 / UNSELECT;
 const float DELETING = 0.08;
 
-void highlight(Object *selected_bin, float factor)
+void highlight(Object* selected_bin, float factor)
 {
   if (selected_bin)
   {
@@ -32,7 +32,7 @@ void highlight(Object *selected_bin, float factor)
   }
 }
 
-int main(int argc, char **argv)
+int main(int argc, char** argv)
 {
   GOOGLE_PROTOBUF_VERIFY_VERSION;
 
@@ -46,7 +46,7 @@ int main(int argc, char **argv)
   auto robotName = getPrivateParameter<std::string>("arm", "virtual-arm");
   ROS_INFO_STREAM("This cell controls arm " << robotName);
 
-  DummyRobotArmController controller{n, NODE_NAME, robotName};
+  DummyRobotArmController controller{ n, NODE_NAME, robotName };
 
   std::map<std::string, CellController> clients;
   std::vector<std::pair<std::string, std::string>> actions;
@@ -62,81 +62,77 @@ int main(int argc, char **argv)
   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";
+  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;
+  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)
-  {
+      "send_scene", 1000, [&controller](const std_msgs::EmptyConstPtr& msg) { controller.sendScene(); });
+  ros::Timer timer = n.createTimer(ros::Duration(3), [&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)
+  });  // send a scene every three 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)
       {
-
-        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;
-        }
-      };
+        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();