diff --git a/src/moveit_sorting_controller.cpp b/src/moveit_sorting_controller.cpp index 793bed6bb8df47123f095492ca764110630d129c..b475dfaf878df616bcefe456f1a859c97ccd9ac9 100644 --- a/src/moveit_sorting_controller.cpp +++ b/src/moveit_sorting_controller.cpp @@ -45,6 +45,8 @@ int main(int argc, char** argv) mqtt_connection->listen(commandTopic); auto other_cell_topic = getPrivateParameter<std::string>("other_cell", "/ceti_cell_2_placeworld/scene/update"); mqtt_connection->listen(other_cell_topic); + auto scene_observer_topic = getPrivateParameter<std::string>("scene_observer", "object_locator/scene/update"); + mqtt_connection->listen(scene_observer_topic); controller.addConnection(std::move(mqtt_connection)); controller.addCallback(other_cell_topic, [&controller](auto msg) { @@ -54,6 +56,13 @@ int main(int argc, char** argv) controller.initScene(scene, false); }); + controller.addCallback(scene_observer_topic, [&controller](auto msg) { + ROS_INFO_STREAM("Updating scene from observer."); + Scene scene; + scene.ParseFromString(msg); + controller.initScene(scene, true); + }); + 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") +