39 int main(
int argc,
char** argv)
41 ros::init(argc, argv,
"display_random_state");
45 for (
int i = 0; i < argc; ++i)
47 if (strcmp(argv[i],
"--valid") == 0)
52 if (strcmp(argv[i],
"--invalid") == 0)
77 ROS_ERROR(
"Planning scene did not load properly, exiting...");
81 std::cout <<
"Type a number and hit Enter. That number of ";
83 std::cout <<
"valid ";
85 std::cout <<
"invalid ";
86 std::cout <<
"states will be randomly generated at an interval of one second and published as a planning scene." 91 for (std::size_t i = 0; i < n; ++i)
96 unsigned int attempts = 0;
105 }
while (!found && attempts < 100);
108 std::cout <<
"Unable to find valid state" << std::endl;
115 unsigned int attempts = 0;
124 }
while (!found && attempts < 100);
127 std::cout <<
"Unable to find invalid state" << std::endl;
134 moveit_msgs::PlanningScene psmsg;
void startSceneMonitor(const std::string &scene_topic=DEFAULT_PLANNING_SCENE_TOPIC)
Start the scene monitor.
int main(int argc, char **argv)
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
const planning_scene::PlanningScenePtr & getPlanningScene()
Avoid this function! Returns an unsafe pointer to the current planning scene.
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.
void startWorldGeometryMonitor(const std::string &collision_objects_topic=DEFAULT_COLLISION_OBJECT_TOPIC, const std::string &planning_scene_world_topic=DEFAULT_PLANNING_SCENE_WORLD_TOPIC, const bool load_octomap_monitor=true)
Start listening for objects in the world, the collision map and attached collision objects...
PlanningSceneMonitor Subscribes to the topic planning_scene.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROSCPP_DECL void shutdown()