39 int main(
int argc,
char **argv)
44 bool full_scene =
false;
47 int filename_index = 1;
49 if (strncmp(argv[1],
"--scene", 7) == 0)
63 pub_scene = nh.
advertise<moveit_msgs::PlanningScene>(
"planning_scene", 1);
65 pub_scene = nh.
advertise<moveit_msgs::PlanningSceneWorld>(
"planning_scene_world", 1);
68 opt.robot_description_ =
"robot_description";
69 opt.load_kinematics_solvers_ =
false;
73 std::ifstream
f(argv[filename_index]);
74 if (f.good() && !f.eof())
76 ROS_INFO(
"Publishing geometry from '%s' ...", argv[filename_index]);
77 ps.loadGeometryFromStream(f);
78 moveit_msgs::PlanningScene ps_msg;
79 ps.getPlanningSceneMsg(ps_msg);
80 ps_msg.is_diff =
true;
83 unsigned int attempts = 0;
84 ROS_WARN(
"Waiting for the subscriber %d", pub_scene.getNumSubscribers());
85 while (pub_scene.getNumSubscribers() < 1 && ++attempts < 100) {
86 ROS_WARN(
"Waiting for the subscriber");
91 pub_scene.publish(ps_msg);
93 pub_scene.publish(ps_msg.world);
98 ROS_WARN(
"Unable to load '%s'.", argv[filename_index]);
101 ROS_WARN(
"A filename was expected as argument. That file should be a text representation of the geometry in a planning scene.");
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int main(int argc, char **argv)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROSCPP_DECL void shutdown()