display_random_state.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
38 
39 int main(int argc, char** argv)
40 {
41  ros::init(argc, argv, "display_random_state");
42 
43  bool valid = false;
44  bool invalid = false;
45  for (int i = 0; i < argc; ++i)
46  {
47  if (strcmp(argv[i], "--valid") == 0)
48  {
49  valid = true;
50  break;
51  }
52  if (strcmp(argv[i], "--invalid") == 0)
53  {
54  invalid = true;
55  break;
56  }
57  }
58 
60  spinner.start();
61 
62  ros::NodeHandle nh;
64  opt.robot_description_ = "robot_description";
65  robot_model_loader::RobotModelLoaderPtr rml(new robot_model_loader::RobotModelLoader(opt));
68  psm.startSceneMonitor();
69  ros::Publisher pub_scene = nh.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
70 
71  ros::Duration(0.5).sleep();
72 
73  do
74  {
75  if (!psm.getPlanningScene())
76  {
77  ROS_ERROR("Planning scene did not load properly, exiting...");
78  break;
79  }
80 
81  std::cout << "Type a number and hit Enter. That number of ";
82  if (valid)
83  std::cout << "valid ";
84  else if (invalid)
85  std::cout << "invalid ";
86  std::cout << "states will be randomly generated at an interval of one second and published as a planning scene."
87  << std::endl;
88  std::size_t n;
89  std::cin >> n;
90 
91  for (std::size_t i = 0; i < n; ++i)
92  {
93  if (valid)
94  {
95  bool found = false;
96  unsigned int attempts = 0;
97  do
98  {
99  attempts++;
100  psm.getPlanningScene()->getCurrentStateNonConst().setToRandomPositions();
103  psm.getPlanningScene()->checkCollision(req, res);
104  found = !res.collision;
105  } while (!found && attempts < 100);
106  if (!found)
107  {
108  std::cout << "Unable to find valid state" << std::endl;
109  continue;
110  }
111  }
112  else if (invalid)
113  {
114  bool found = false;
115  unsigned int attempts = 0;
116  do
117  {
118  attempts++;
119  psm.getPlanningScene()->getCurrentStateNonConst().setToRandomPositions();
122  psm.getPlanningScene()->checkCollision(req, res);
123  found = res.collision;
124  } while (!found && attempts < 100);
125  if (!found)
126  {
127  std::cout << "Unable to find invalid state" << std::endl;
128  continue;
129  }
130  }
131  else
132  psm.getPlanningScene()->getCurrentStateNonConst().setToRandomPositions();
133 
134  moveit_msgs::PlanningScene psmsg;
135  psm.getPlanningScene()->getPlanningSceneMsg(psmsg);
136  pub_scene.publish(psmsg);
137  std::cout << psm.getPlanningScene()->getCurrentState() << std::endl;
138 
139  sleep(1);
140  }
141  } while (nh.ok());
142 
143  ros::shutdown();
144  return 0;
145 }
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
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void spinner()
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()
bool ok() const
#define ROS_ERROR(...)


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jul 18 2018 02:49:00