55int main(
int argc,
char** argv)
57 rclcpp::init(argc, argv);
58 auto node = rclcpp::Node::make_shared(
"publish_planning_scene");
62 bool full_scene =
false;
65 int filename_index = 1;
68 if (strncmp(argv[1],
"--scene", 7) == 0)
77 rclcpp::executors::MultiThreadedExecutor executor;
78 executor.add_node(node);
80 rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr pub_scene;
81 rclcpp::Publisher<moveit_msgs::msg::PlanningSceneWorld>::SharedPtr pub_world_scene;
85 pub_scene = node->create_publisher<moveit_msgs::msg::PlanningScene>(
90 pub_world_scene = node->create_publisher<moveit_msgs::msg::PlanningSceneWorld>(
98 auto rml = std::make_shared<robot_model_loader::RobotModelLoader>(node, opt);
101 std::ifstream f(argv[filename_index]);
104 RCLCPP_INFO(node->get_logger(),
"Publishing geometry from '%s' ...", argv[filename_index]);
105 moveit_msgs::msg::PlanningScene ps_msg;
107 ps_msg.is_diff =
true;
109 unsigned int attempts = 0;
110 while (pub_scene->get_subscription_count() < 1 && ++attempts < 100)
111 rclcpp::sleep_for(500ms);
115 pub_scene->publish(ps_msg);
119 pub_world_scene->publish(ps_msg.world);
122 rclcpp::sleep_for(1s);
127 RCLCPP_WARN(node->get_logger(),
128 "A filename was expected as argument. That file should be a text representation of the geometry in a "
int main(int argc, char **argv)