listen for scenes in another cell

src/moveit_sorting_controller.cpp

@@ -33,7 +33,10 @@ int main(int argc, char **argv)
 
   ros::NodeHandle n("connector_node_ros_ccf"); // namespace where the connection_address is
 
-  MoveItRobotArmController controller{n, NODE_NAME, "arm1"};
+  auto robotName = getPrivateParameter<std::string>("arm", "arm1");
+  ROS_INFO_STREAM("This cell controls arm " << robotName);
+
+  MoveItRobotArmController controller{n, NODE_NAME, robotName};
 
   auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883");
   std::unique_ptr<MqttConnection>
@@ -42,8 +45,16 @@ int main(int argc, char **argv)
   auto commandTopic = getParameter<std::string>(n, "topics/command", "/ceti_cell_placeworld/command");
   mqtt_connection->listen(commandTopic);
   controller.setCommandTopic(commandTopic);
+  auto other_cell_topic = getPrivateParameter<std::string>("other_cell", "/ceti_cell_2_placeworld/scene/update");
+  mqtt_connection->listen(other_cell_topic);
   controller.addConnection(std::move(mqtt_connection));
 
+  controller.addCallback(other_cell_topic, [&controller](auto msg) {
+    Scene scene;
+    scene.ParseFromString(msg);
+    controller.initScene(scene, false);
+  });
+
   ros::WallDuration(5).sleep(); // wait 5 secs to init scene (to give moveit time to load)
 
   controller.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +