Add --world flag to publish_scene_from_text

moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp

@@ -43,6 +43,8 @@
 #include <rclcpp/publisher.hpp>
 #include <rclcpp/version.h>
 #include <moveit/utils/logger.hpp>
+#include <boost/program_options/parsers.hpp>
+#include <boost/program_options/variables_map.hpp>
 #if RCLCPP_VERSION_GTE(20, 0, 0)
 #include <rclcpp/event_handler.hpp>
 #else
@@ -58,75 +60,104 @@ int main(int argc, char** argv)
   auto node = rclcpp::Node::make_shared("publish_planning_scene");
   moveit::setNodeLoggerName(node->get_name());
 
-  // decide whether to publish the full scene
-  bool full_scene = false;
+  bool publish_scene = false;  // Set by --scene flag
+  bool publish_world = false;  // Set by --world flag, or the default if no flags specified
+  std::string filename;
 
-  // the index of the argument with the filename
-  int filename_index = 1;
-  if (argc > 2)
+  boost::program_options::options_description desc;
+  desc.add_options()("scene", "Publish full planning scene")("world", "Publish world scene (default)");
+  desc.add_options()("input-file", boost::program_options::value<std::string>(&filename), "input file");
+
+  boost::program_options::positional_options_description p;
+  p.add("input-file", -1);
+
+  boost::program_options::variables_map vm;
+  boost::program_options::parsed_options po =
+      boost::program_options::command_line_parser(argc, argv).options(desc).positional(p).run();
+  boost::program_options::store(po, vm);
+  boost::program_options::notify(vm);
+
+  if (vm.count("scene"))
   {
-    if (strncmp(argv[1], "--scene", 7) == 0)
-    {
-      full_scene = true;
-      filename_index = 2;
-    }
+    publish_scene = true;
+  }
+  if (vm.count("world"))
+  {
+    publish_world = true;
+  }
+
+  // If no flags specified, default to world (backwards compatibility)
+  if (!vm.count("scene") && !vm.count("world"))
+  {
+    publish_world = true;
   }
 
-  if (argc > 1)
+  if (filename.empty())
   {
-    rclcpp::executors::MultiThreadedExecutor executor;
-    executor.add_node(node);
+    RCLCPP_WARN(node->get_logger(),
+                "A filename was expected as argument. That file should be a text representation of the geometry in a "
+                "planning scene.");
+    return 1;
+  }
 
-    rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr pub_scene;
-    rclcpp::Publisher<moveit_msgs::msg::PlanningSceneWorld>::SharedPtr pub_world_scene;
+  rclcpp::executors::MultiThreadedExecutor executor;
+  executor.add_node(node);
 
-    if (full_scene)
+  rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr pub_scene;
+  rclcpp::Publisher<moveit_msgs::msg::PlanningSceneWorld>::SharedPtr pub_world_scene;
+
+  if (publish_scene)
+  {
+    pub_scene = node->create_publisher<moveit_msgs::msg::PlanningScene>(
+        planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC, 1);
+  }
+  if (publish_world)
+  {
+    pub_world_scene = node->create_publisher<moveit_msgs::msg::PlanningSceneWorld>(
+        planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, 1);
+  }
+
+  robot_model_loader::RobotModelLoader::Options opt;
+  opt.robot_description = "robot_description";
+  opt.load_kinematics_solvers = false;
+
+  auto rml = std::make_shared<robot_model_loader::RobotModelLoader>(node, opt);
+  planning_scene::PlanningScene ps(rml->getModel());
+
+  std::ifstream f(filename);
+  if (ps.loadGeometryFromStream(f))
+  {
+    RCLCPP_INFO(node->get_logger(), "Publishing geometry from '%s' ...", filename.c_str());
+    moveit_msgs::msg::PlanningScene ps_msg;
+    ps.getPlanningSceneMsg(ps_msg);
+    ps_msg.is_diff = true;
+
+    // Wait for subscribers on both topics
+    unsigned int attempts = 0;
+    while (++attempts < 100)
     {
-      pub_scene = node->create_publisher<moveit_msgs::msg::PlanningScene>(
-          planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC, 1);
+      bool scene_ready = !publish_scene || pub_scene->get_subscription_count() >= 1;
+      bool world_ready = !publish_world || pub_world_scene->get_subscription_count() >= 1;
+      if (scene_ready && world_ready)
+        break;
+      rclcpp::sleep_for(500ms);
     }
-    else
+
+    if (publish_scene)
     {
-      pub_world_scene = node->create_publisher<moveit_msgs::msg::PlanningSceneWorld>(
-          planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, 1);
+      pub_scene->publish(ps_msg);
     }
-
-    robot_model_loader::RobotModelLoader::Options opt;
-    opt.robot_description = "robot_description";
-    opt.load_kinematics_solvers = false;
-
-    auto rml = std::make_shared<robot_model_loader::RobotModelLoader>(node, opt);
-    planning_scene::PlanningScene ps(rml->getModel());
-
-    std::ifstream f(argv[filename_index]);
-    if (ps.loadGeometryFromStream(f))
+    if (publish_world)
     {
-      RCLCPP_INFO(node->get_logger(), "Publishing geometry from '%s' ...", argv[filename_index]);
-      moveit_msgs::msg::PlanningScene ps_msg;
-      ps.getPlanningSceneMsg(ps_msg);
-      ps_msg.is_diff = true;
-
-      unsigned int attempts = 0;
-      while (pub_scene->get_subscription_count() < 1 && ++attempts < 100)
-        rclcpp::sleep_for(500ms);
-
-      if (full_scene)
-      {
-        pub_scene->publish(ps_msg);
-      }
-      else
-      {
-        pub_world_scene->publish(ps_msg.world);
-      }
-
-      rclcpp::sleep_for(1s);
+      pub_world_scene->publish(ps_msg.world);
     }
+
+    rclcpp::sleep_for(1s);
   }
   else
   {
-    RCLCPP_WARN(node->get_logger(),
-                "A filename was expected as argument. That file should be a text representation of the geometry in a "
-                "planning scene.");
+    RCLCPP_ERROR(node->get_logger(), "Failed to load geometry from file '%s'", filename.c_str());
+    return 1;
   }
 
   rclcpp::shutdown();