Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
00038
00039 int main(int argc, char** argv)
00040 {
00041 ros::init(argc, argv, "display_random_state");
00042
00043 bool valid = false;
00044 bool invalid = false;
00045 for (int i = 0; i < argc; ++i)
00046 {
00047 if (strcmp(argv[i], "--valid") == 0)
00048 {
00049 valid = true;
00050 break;
00051 }
00052 if (strcmp(argv[i], "--invalid") == 0)
00053 {
00054 invalid = true;
00055 break;
00056 }
00057 }
00058
00059 ros::AsyncSpinner spinner(1);
00060 spinner.start();
00061
00062 ros::NodeHandle nh;
00063 robot_model_loader::RobotModelLoader::Options opt;
00064 opt.robot_description_ = "robot_description";
00065 robot_model_loader::RobotModelLoaderPtr rml(new robot_model_loader::RobotModelLoader(opt));
00066 planning_scene_monitor::PlanningSceneMonitor psm(rml);
00067 psm.startWorldGeometryMonitor();
00068 psm.startSceneMonitor();
00069 ros::Publisher pub_scene = nh.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
00070
00071 ros::Duration(0.5).sleep();
00072
00073 do
00074 {
00075 if (!psm.getPlanningScene())
00076 {
00077 ROS_ERROR("Planning scene did not load properly, exiting...");
00078 break;
00079 }
00080
00081 std::cout << "Type a number and hit Enter. That number of ";
00082 if (valid)
00083 std::cout << "valid ";
00084 else if (invalid)
00085 std::cout << "invalid ";
00086 std::cout << "states will be randomly generated at an interval of one second and published as a planning scene."
00087 << std::endl;
00088 std::size_t n;
00089 std::cin >> n;
00090
00091 for (std::size_t i = 0; i < n; ++i)
00092 {
00093 if (valid)
00094 {
00095 bool found = false;
00096 unsigned int attempts = 0;
00097 do
00098 {
00099 attempts++;
00100 psm.getPlanningScene()->getCurrentStateNonConst().setToRandomPositions();
00101 collision_detection::CollisionRequest req;
00102 collision_detection::CollisionResult res;
00103 psm.getPlanningScene()->checkCollision(req, res);
00104 found = !res.collision;
00105 } while (!found && attempts < 100);
00106 if (!found)
00107 {
00108 std::cout << "Unable to find valid state" << std::endl;
00109 continue;
00110 }
00111 }
00112 else if (invalid)
00113 {
00114 bool found = false;
00115 unsigned int attempts = 0;
00116 do
00117 {
00118 attempts++;
00119 psm.getPlanningScene()->getCurrentStateNonConst().setToRandomPositions();
00120 collision_detection::CollisionRequest req;
00121 collision_detection::CollisionResult res;
00122 psm.getPlanningScene()->checkCollision(req, res);
00123 found = res.collision;
00124 } while (!found && attempts < 100);
00125 if (!found)
00126 {
00127 std::cout << "Unable to find invalid state" << std::endl;
00128 continue;
00129 }
00130 }
00131 else
00132 psm.getPlanningScene()->getCurrentStateNonConst().setToRandomPositions();
00133
00134 moveit_msgs::PlanningScene psmsg;
00135 psm.getPlanningScene()->getPlanningSceneMsg(psmsg);
00136 pub_scene.publish(psmsg);
00137 std::cout << psm.getPlanningScene()->getCurrentState() << std::endl;
00138
00139 sleep(1);
00140 }
00141 } while (nh.ok());
00142
00143 ros::shutdown();
00144 return 0;
00145 }