visualize_robot_collision_volume.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 #include <cstdlib>
40 #include <memory>
41 
42 static const std::string ROBOT_DESCRIPTION = "robot_description";
43 
44 int main(int argc, char** argv)
45 {
46  ros::init(argc, argv, "visualize_robot_collision_volume");
47 
49  spinner.start();
50 
51  double radius = 0.02;
52  double lifetime = 600.0;
53  ros::NodeHandle nh;
54 
55  std::shared_ptr<tf2_ros::Buffer> tf_buffer = std::make_shared<tf2_ros::Buffer>();
56  std::shared_ptr<tf2_ros::TransformListener> tf_listener =
57  std::make_shared<tf2_ros::TransformListener>(*tf_buffer, nh);
59  if (psm.getPlanningScene())
60  {
61  psm.startWorldGeometryMonitor();
62  psm.startSceneMonitor();
63  psm.startStateMonitor();
64  ros::Publisher pub_markers = nh.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 10);
65  std::cout << "\nListening for planning scene...\nType the number of spheres to generate and press Enter: "
66  << std::endl;
67  int N;
68  std::cin >> N;
69 
70  planning_scene::PlanningScenePtr scene = psm.getPlanningScene();
71  std::vector<double> aabb;
72  scene->getCurrentState().computeAABB(aabb);
73 
74  // publish the bounding box
75  visualization_msgs::Marker mk;
76  mk.header.stamp = ros::Time::now();
77  mk.header.frame_id = scene->getPlanningFrame();
78  mk.ns = "bounding_box";
79  mk.id = 0;
80  mk.type = visualization_msgs::Marker::CUBE;
81  mk.action = visualization_msgs::Marker::ADD;
82  mk.pose.position.x = (aabb[0] + aabb[1]) / 2.0;
83  mk.pose.position.y = (aabb[2] + aabb[3]) / 2.0;
84  mk.pose.position.z = (aabb[4] + aabb[5]) / 2.0;
85  mk.pose.orientation.w = 1.0;
86  mk.scale.x = aabb[1] - aabb[0];
87  mk.scale.y = aabb[3] - aabb[2];
88  mk.scale.z = aabb[5] - aabb[4];
89  mk.color.r = 0.0f;
90  mk.color.g = 0.5f;
91  mk.color.b = 1.0f;
92  mk.color.a = 0.3f;
93  mk.lifetime = ros::Duration(lifetime);
94  visualization_msgs::MarkerArray arr;
95  arr.markers.push_back(mk);
96  pub_markers.publish(arr);
97 
98  Eigen::Isometry3d t;
99  t.setIdentity();
100  std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> points;
101  std::size_t published = 0;
104 
105  std_msgs::ColorRGBA color;
106  color.r = 1.0f;
107  color.g = 0.0f;
108  color.b = 0.0f;
109  color.a = 1.0f;
110 
111  for (int i = 0; i < N; ++i)
112  {
113  t.translation() = Eigen::Vector3d(rng.uniformReal(aabb[0], aabb[1]), rng.uniformReal(aabb[2], aabb[3]),
114  rng.uniformReal(aabb[4], aabb[5]));
115  scene->getWorldNonConst()->clearObjects();
116  scene->getWorldNonConst()->addToObject("test", shapes::ShapeConstPtr(new shapes::Sphere(radius)), t);
118  scene->checkCollision(req, res);
119  if (res.collision)
120  {
121  points.push_back(t.translation());
122  if (points.size() - published >= 100 || (points.size() > published && i + 1 >= N))
123  {
124  arr.markers.clear();
125  for (std::size_t k = published; k < points.size(); ++k)
126  {
127  visualization_msgs::Marker mk;
128  mk.header.stamp = ros::Time::now();
129  mk.header.frame_id = scene->getPlanningFrame();
130  mk.ns = "colliding";
131  mk.id = k;
132  mk.type = visualization_msgs::Marker::SPHERE;
133  mk.action = visualization_msgs::Marker::ADD;
134  mk.pose.position.x = points[k].x();
135  mk.pose.position.y = points[k].y();
136  mk.pose.position.z = points[k].z();
137  mk.pose.orientation.w = 1.0;
138  mk.scale.x = mk.scale.y = mk.scale.z = radius;
139  mk.color = color;
140  mk.lifetime = ros::Duration(lifetime);
141  arr.markers.push_back(mk);
142  pub_markers.publish(arr);
143  }
144  pub_markers.publish(arr);
145  published = points.size();
146  }
147  }
148  }
149  }
150 
151  return 0;
152 }
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
geometry_msgs::TransformStamped t
EIGEN_MAKE_ALIGNED_OPERATOR_NEW void clear()
void spinner()
void publish(const boost::shared_ptr< M > &message) const
PlanningSceneMonitor Subscribes to the topic planning_scene.
double uniformReal(double lower_bound, double upper_bound)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
tf2_ros::Buffer * tf_buffer
static const std::string ROBOT_DESCRIPTION
int main(int argc, char **argv)
static Time now()
std::shared_ptr< const Shape > ShapeConstPtr


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sat Jul 20 2019 03:55:44