From 09e0dd4ed4b01c646b167ff75e3e2c9446b686a8 Mon Sep 17 00:00:00 2001 From: LetianLi Date: Wed, 25 Jun 2025 14:14:14 -0700 Subject: [PATCH 1/2] Add --world flag --- .../src/publish_scene_from_text.cpp | 137 +++++++++++------- 1 file changed, 84 insertions(+), 53 deletions(-) diff --git a/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp b/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp index 5f4d4194a2..54822cede1 100644 --- a/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp +++ b/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp @@ -43,6 +43,8 @@ #include #include #include +#include +#include #if RCLCPP_VERSION_GTE(20, 0, 0) #include #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(&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::SharedPtr pub_scene; - rclcpp::Publisher::SharedPtr pub_world_scene; + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); - if (full_scene) + rclcpp::Publisher::SharedPtr pub_scene; + rclcpp::Publisher::SharedPtr pub_world_scene; + + if (publish_scene) + { + pub_scene = node->create_publisher( + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC, 1); + } + if (publish_world) + { + pub_world_scene = node->create_publisher( + 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(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( - 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( - 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(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(); From 917fe597effe91926922a86564320fb263c6ff11 Mon Sep 17 00:00:00 2001 From: LetianLi <45277806+LetianLi@users.noreply.github.com> Date: Thu, 26 Jun 2025 11:52:23 -0500 Subject: [PATCH 2/2] Use publish_blank variables instead of recounting args again Co-authored-by: Robert Haschke --- .../planning_components_tools/src/publish_scene_from_text.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp b/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp index 54822cede1..daf4ba458e 100644 --- a/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp +++ b/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp @@ -87,7 +87,7 @@ int main(int argc, char** argv) } // If no flags specified, default to world (backwards compatibility) - if (!vm.count("scene") && !vm.count("world")) + if (!publish_scene && !publish_world) { publish_world = true; }