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>(
66 pub_scene = nh.
advertise<moveit_msgs::PlanningSceneWorld>(
71 opt.load_kinematics_solvers_ =
false;
75 std::ifstream
f(argv[filename_index]);
76 if (f.good() && !f.eof())
78 ROS_INFO(
"Publishing geometry from '%s' ...", argv[filename_index]);
79 ps.loadGeometryFromStream(f);
80 moveit_msgs::PlanningScene ps_msg;
81 ps.getPlanningSceneMsg(ps_msg);
82 ps_msg.is_diff =
true;
85 unsigned int attempts = 0;
86 while (pub_scene.getNumSubscribers() < 1 && ++attempts < 100)
90 pub_scene.publish(ps_msg);
92 pub_scene.publish(ps_msg.world);
97 ROS_WARN(
"Unable to load '%s'.", argv[filename_index]);
100 ROS_WARN(
"A filename was expected as argument. That file should be a text representation of the geometry in a "
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
static const std::string DEFAULT_PLANNING_SCENE_WORLD_TOPIC
std::string robot_description_
The string name corresponding to the ROS param where the URDF is loaded; Using the same parameter nam...
Structure that encodes the options to be passed to the RobotModelLoader constructor.
static const std::string DEFAULT_PLANNING_SCENE_TOPIC
The name of the topic used by default for receiving full planning scenes or planning scene diffs...
int main(int argc, char **argv)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROSCPP_DECL void shutdown()